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Preface . 

The  Kalman  Filter  is  a  minimum  variance  filter  derived 
with  the  following  assumptions:  the  dynamics  of  the  system 
are  linear,  the  observations  are  linear  functions  of  the 
states,  and  all  of  the  noise  sources  and.,  their  statistical 

f 

characteristics  are  known.  For  the  case' of  estimating  the' 
state  of  the  ballistic  re-entry  vehicle  on  the  basis  of 
noisy  measurements,  the  Kalman  theory  cennot  be  applied 
directly.  The  validity  of  the  linearizations  made  in  the 
extension  of  the  Kalman  Filter  arc  examined .  , -  , 

VJc  v;ish  to  express  our  indebtedness  tc  Lt.  Col.  Roacr 
W.  Johnson  our  thesis  advisor  for  his  continual  encourage¬ 
ment,  advice,  and  patience  throughout  this  study. 
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Abstract 


This  thesis  presents  the  results  of  a  study  wherein 
the  Kalman  filtering  technique  is  applied  to  the  estimation 
and  prediction  of  the  trajectory  of  a  ballistic  missile 

•  v 

from  radar  measurements  made  from  an  airborne  radar  system. 

Any  intercept  system  which  is  to  guide  an  anti-missile  is 

critically  dependent  on  these  computational  functions. 

$  ° 

The  Kalman  Filter  equations  are  based  on  a  number  of 
assumptions  that  are  not  entirely  justified  in  actual  prac¬ 
tice.  For  the  case  of  estimating  the  state  of  a  ballistic 
re-entry  vehicle  on  the  basis  of  noisy  measurements,  the 
Kalman  theory  cannot  be  applied  directly. 

In  this  paper  the  Kalman  estimator  is  extended  to  non- 

*  '■  e  * 

linear  trajectory  equations  and  unknown  ballistic  para¬ 
meters.  An  estimation  and  prediction  model  is  developed 
.assuming  that  azimuth,  elevation,  range  and  range-rate  data 


is  provided  from  a  phased^sray  radar  aboard  an  aircraft. 

In  order  to  evaluatfcthemodel ,  a  digital  computer  program 
was  developed  wherein  a  reference  trajectory  for  a  missile 
is  generated  and  this  information,  along  with  tracker  air¬ 
craft  position,  is  used  by  a  radar  model  to  generate  air¬ 
borne  tracking  information  which  is  contaminated  with  noise. 
From  this  information  the  Kalman  estimation  and  prediction 
model  yields  estimates  of  the  present  states  and  future 
states  of  the  target.  These  are  compared  with  the  refer¬ 
ence  trajectory  to  evaluate  the  model. 
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APPLICATION  OF  THE  EXTENDED  KALMAN  FILTER 
TO  BALLISTIC  TRAJECTORY  ESTIMATION  AND  PREDICTION 


I .  INTRODUCTION 


This  study  is  concerned  with  the  computational  aspects 

of  an  airborne  radar  system  which  tracks  re-entry  vehicles. 

e*. 

It  is  required  that  position  and  velocity  of  an  incoming  re¬ 
entry  vehicle  be  .determined  from  noisy  radar  data.  Further¬ 
more,  it  is  necessary  to  predict  the  vehicle's  future  posi¬ 
tion  on  the  basis  of^th^j^esent  estimate  of  position  and 
velocity.  The  first  part  off  this  problem  is  referred  to  as 
the  "estimation  problem",  whereas  the  second  part  is  re¬ 
ferred  to  as  the  "prediction  problem".  A  third  aspect  of 
the  problem  is  “identification".  Identification  differs 
slightly  from  estimation  in  the  sense  that  the  imperfectly 
known  parameters  (e.g.,  ballistic  coefficient)  character¬ 
izing  the  signal-generating  process  are  obtained  from  noisy 
observations,  whereas  previously  the  state  variables  (i.e., 
position  and  velocity  coordinates)  were  estimated.  Know¬ 
ledge  of  the  ballistic  coefficient  significantly  ehhances 

v 

the  quality  of  the  prediction. 

In  the  usual  trajectory  determination  problem,  we  make 

* 

discrete. noisy  measurements  of  variables  related  to  the 

i  v  Q 

state  of  a  vehicle  whose  motion  is  uniquely  determined  by  '[ 
its  unknown?  initial  state,  and  we  ask,  on  the  ba£is  of  noisy 
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measurement^ ,  for  the  "best"  estimate  of  the  state  at  any 
time.  In  a  series  of  well-known  papers  (Ref  1,2,3) 

r.E.  Kalman  describes  an  optimal  filter  applicable  to  noisy, 

..  *  , 

time-varying,  linear’  systems.  This  filter,  which  is  essen¬ 
tially  a  minimum  variance  linear  estimator,  is  particularly 
,  suitable  for  trajectory  determination  problems  in  which 
estimates  of  state  variables  are  desired  as  rapidly-as  pos¬ 
sible.  However,  the  trajectory  estimation  problem  is, non- 

V 

o  -  ' 

linear  and  the  Kalman  theory  cannot  be  applied  directly. 

.  t  % 

y  W  j 

Although  the  Kalman  filter  is  optimum  only  when  the 

v  o  ■  •  : 

system  differential  equations  and  measurements  are  linear, 

*  -  a. 

it  has  found  considerable  use  in  estimating  the  state  vari- 

«  f  *  ‘ 

ables  of  a  nonlinear  system  with  measurements  that  are  noise- 

/  1 

4  corrupted  nonlinear  functions  of  state  variables.  This  em¬ 
ployment  of  the  Kalman  filter  is  .frequently  referred  to  as 

*  o> 

the  "Extended  Kalman  Filter"  C  it  is  an  intuitive  but  fre¬ 
quently  successful  application  of  the  Kalman  filter  in  the 
absence. of  truly  optimum  filters  for  non-linear  systems. 

**  *  f  ,  • 

1^7  brief,  the  Kalman  Filter  can  be  quite  useful  in  esti¬ 
mating  the  state  variables  of  nonlinear  systems.  However, 

:  X*  •  4 

’  more  care  must  be  exercised  in  checking  theoretical  results 

O  ’  •  \ 

by  means  of  simulation.  When  thesKalman  Filter  produces  poor 
estimates  of  the  states  ^of  a  nonlinear  system,  ingenious,-' 
changes  can  often  produce  a  useful  modified  version. 

*'  t  * 
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II.  FILTER  EQUATIONS  / 

c 

v  ‘  <. 

v  »  '• 

The  Linear,  -  Gaussian  Case  ^ _ 

The  Kalman  Filter  equations. specify  an  estimate  of  the 
state  of  a  linear  time-Varying  dynamical  system  observed  se- 
qentially  in  jthe  presence  of  additive  white  Gaussian  noise. 
The  equations  used  in  the  -Kalman  Filter  are  giyen  belovr. 

The  derivation  of0  these  equations  can  be  found  in  numerous 
references  (Ref  1/2) .  The  linear  system  is,  described  by 


X  -  F  X  '+  0 


U) 


where  the  components  of  X  dre  the  states  of  the  system;  F  is 
the  system  description  matrix;  and  U  is  a  white  Gaussian 

.  a  ■** 

noise  process  that  may  represent  either  actual  input  noise 

1  ■  ,  '  -  .  I  ■ 

or  inaccuracies  in  the. system  model.  Observations  repre- 

„  "  •  i  /  ‘ 

sented  by  the  vector  Z  axe  mad6  according  to  . 


*  M  X  +  V 


(2) 


where  M,  the  measurement  matrix,  describes  the  linear,  combi¬ 
nation  of  the'  state  variables  which  comprise  Z  in  theab-  ^ 
sence  of  noise,,  and  V  is?  a  white  Gaussian  noise  process  as¬ 
sumed-independent  of  U.  The  covariances  of  U  and  V  are  de- 

mm  b  mm 

noted  Q  and  R  respectively,  and  it  is  assumed  that  an  a 

I  •  '  s. 

priori  estimate  of  states,  X  has  been  made  with  error  co- 
va~riance  P. 
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The  filtering  equations  may  be  written  as  a  set  of  pre¬ 


diction  equations 


W->  -  *&<♦> 


*=  ♦  Pk(+)  ±  +  Q 


/  v 

/  (3) 

/ 

/ 

;/  (4) 


which  describes  the  behavior  of  the  estimate  and  its  error 
covariance  betv/een  observations,  and  a  set  of  correction/ 
equations  -  /  1 '  '  "  ' 


X  (+)  =  X  (-)  +  K  [Z  -  H  X  <-)]f  /  (5) 

''  *  .  „ 

•  K  *=  P  (-)  MT  [M  p  (-)  mt  +  RJ/1  (6) 

-  v  '  *  / 

•  •  P  (+)  -  (I  -  K/M]  VI-)/  '  '  (7) 

^  7-  -  .  <  /  •  .  v  .. 

_  _  _ _ _ ^  I  * 

which  take  into  account  the  iast^d^servation  Z.  The(-) 
and  (+)  indicate  immediately  brier  to  and  after  measurements, 
and  ♦  is  the  state  transition  matrist  of  equation  (1).  given  by 


♦.(At)  »  *  I  *  F  At  +  F*  At*  +  ..  .  .  (8) 

*'  v  * 

t 

Data  Needed  for  Kalman  Filtering.  Inorder  to  employ  the 


1  -2  .  .2 


Kalman  filtering  process  certain  information  about  the  sys¬ 
tem  and  the  statistical  characteristics  of _ the  input  and 

£ 

measurement  noises  must  be  known  or  assumed.  The  following 

•  '  s'  ,,  S 

data  is  required  before  the  Kalman  filtering  propess  can  be' 
initiated:  / 


/ 


r  " 


i 
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1.  System  description  or  F  matrix  for  all  values  of  time. 

2.  Sampling  time  At* 

3.  State  transition  matrix  £  (At).  . 

4.  Measurement  matrix  M. 

5.  Measurement  noise  covariance  matrix  R.  .  v 

6.  Input  noise  covariance 

4  7 .  Initial  state  covarianct 

/  >  A 

/  8.  Initial  state  estip^te  matrix  X_ (-) . 

.  /  ““"O  ^  i 

Iterative  Procedure.  /The  following  is  the  iterative  pro¬ 
cedure  for  processing  the  Kalman  Filter. 

1.  Compute  ^state  transition  matrix  (At),  Eq  'XQ) . 

2.  Update  state  covariance  matrix  Pjt+^(*")#  Eq  (4) ,  using 
l(At) ,  Pk  (+) ,and  Q. 

'3..  Compute  the  filter  gain  matrix  K,  Eq  (6),  using  M,  P 
(-) ,  and  R.  * 

//  v  -  ° 

4.  Compute  estimate  of  state  X(+) ,  Eq  (5) ,  using  the  ob¬ 
servation  M,  and  $(-).*  | 

/  5.  Update  the  state  covariance  matrix  P(+) ,  Eq  (7). 

t  -■  1 

.  *  • 

6.  The  above  computational  process  is  repeated  each  At 

time  interval,  N 

The  Extended  Hainan  Filter  j  -  . 

The  Kalman  filter  is  a  r/inimum  variance  filter  derived 
with  the  follov/ir.g  assumptions: 

\ 

1.  The  dynamics  of  the  system  are  linear. 

2.  The  observations  are  linear  functions  of  the  states. 

3.  All  of  the  noise  sources  and  their  statistical  char- 

.  / 

acteristics  are  known  . 


t 

# 
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V 


For  the  case  of  estimating  the  state  of  a  ballistic  re¬ 
entry  vehicle  on  the  basis  of  noisy  measurements ,  the  Hai¬ 
nan  theory  cannot  be  applied  directly.  The  system  equa¬ 
tions  governing  the  vehicle  are  highly  non-linear,  and  the 
observation  equation  is  non-linear. 

If  our  knowledge  of  the  system  state  is  such  that  the 
matrices 

c  f 

F  =  — 

3x 

?  — 

x  3m 

M  *  -  (10) 

3x 

—  A 

“  / 

are  approximately  constant  over  the  range  of  uncertainty  in 

« 

X,  then  the  state  transition  matrix,  ±,  can  be  determined 
from  equation  (8)  and  the  filter  gain  calculated  using  the 
redefined  F  and  M  matrices.  It  should  be  noted  that  F  and 
M  matrices  computed  from  equations  (9)  and  (10)  can  be  non¬ 
linear  functions  of  X. 

//'^These  techniques  are  only  approximate.  They  require 

,  '  't,  k 

that  the  disturbances,  measurement  noises, and  uncertainties 

/ 

in  the  state  be  such  a  size  that  the  higher  order  terras  ig¬ 
nored  in  computing  the  error  covariance  are  insignificant. 
..If  this  condition  is  not  satisfied,  the  application  of  the 
Kalman  Filter  to  nonlinear  systems  may  be  useless.  Care 

•  i 

must  be  exercised  in  checking  theoretical  results  by  means 
of  simulation.  Because  the  error  covariance  equations 
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provide  only  an  approximate  evaluation  of  the  estimation 
error  statistics,  Monte  Carlo  techniques  are  required  to 
verify  the  use  of  the  Extended  Kalman  Pilter  for  nonlinear 
systems. 


e 


V' 


£> 
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«f  .  ''  ' 

III.  EQUATIONS  FOR  ESTIMATION  OF  A  BALLISTIC  TRAJECTORY 
Coordinate  System 

The  problem  of  predicting  the  trajectory  of  a  ballistic-, 
vehicle  can  be^foroulated  in  several  ways.'  Foremost  in  any 
formulation  is  the  choice  of  a  dynamically  and  computation¬ 
ally  convenient  frame  of  reference  in  which  to  perform  the 

>,  o 

operations  and  solve  the  problem.  A  logical  choice  to  sat- 

/  o 

isfy  this  requirement  is  a  reference  frame  which’  is  fixed 

with  respect  to  the  earth.  The  coordinate  system  •  chbsen 

i 

has  the  origin  at  the  center  of  the  earth  and  a  vertical 

»  <5  •, 

*  a*  ^  * 

axis  passing  through  the  point  of  acquisition  of  the  target. 

$ '  *  c 

One  level  axis  is  down-range  and  the  other  level  axis  is  in 
•  a  lateral  direction.  This  system  is  essentially  a  tangent- 
plane  ' coordinate  system  fixed- on  the  acquisition  point.  The 
tangent-plane  coordinate  system  has  the  advantage  that  two 
of  its  axes  are  physically  oriented  to  be  nominally, in  the 
missile  flight  plane.  The  initial  covariance  matrix  of 
estimation  error  may  be  more  easily  defined  and  more  gen¬ 
erally  applicable  to  all  acquisition  geometries.  The  main 
>  *  <• 

'  disadvantage  of  the  tangent-plane  system  is  that  more  com- 

,  *  < 

putations  are  performed  during  filtering  to  place  vectors 
on  this  frame.  The  tangent-plane  coordinate  system  is  shown 
in  Figure  3  and  discussed  in  more  detail  in  this  chapter. 
Equations  of  Motion 

Once  a  reference  frame  is  chosen  it  is  necessary  to  for- 
mulate  the  dynamic  equations  of  motion  for  a  ballistic 
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vehicle  on  these  axes.  The  equations  of  notion  for  the  ve¬ 
hicle  in  the  tangent-plane  coordinate  system  are 


*• 

-  2 tV  -  •**> 

R 

-  *>x  IujjX  +  wyY  +  »ZZ]  +  fi2X 


•  r?  *  5"vt i  -  2  K*  -  •**■> 

R  ° 

wy  [uxX  +  uyY  +  bigZ]  +  D2Y 


2  =  -  i|  -  Ipyi  2  -  21V  -  «YXI 

R 


-  »z  t«xX  +  V  +  *zzl  +  “  z 


(11) 


(12) 


(13) 


where  the  symbols  are  defined  in  Table  I.  • 
The  state  vector  has  seven  components:  ’ 


X 

y 


Z 
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TABLE  I 

NOMENCLATURE  FOR  VEHICLE  EQUATIONS  OF  MOTION 

X  -  Down- Range  coordinate  of  vehicle 
y  -  Cross-Range  coordinate  of  vehicle 
Z  -  Vertical  coordinate  of  vehicle’ 

1/  2  2  2* 

Distance  from  center  of  earth  **  jrx  +Y  +z 


xTi  TF  .2 
=  Kfc  +  y  +  z 


R 

V  -  Speed  of  vehicle 
6  -  Ballistic  coefficient  of  vehicle  = 
P 
p 
fi 


w 

CpA 


Atmospheric  density 
Gravitational  constant 
Earth  rate 

Tangent-plane  components  of  earth  rate 


Choice  of  Filter  States 

Once  the  linearized  model  is1  determined,  it  is  neces¬ 
sary  to  choose  what  quantities  are  to  be  estimated  by  the 
filter.  Since  the  errors  in  the  states  of  a  nonlinear  sys¬ 
tem  behave  much  more  linearly  than  the  states  themselves,  it 
was  decided  to  apply  the  linear  filter  theory  only  to  the 
estimates  of  the  errors  in  the  states.  Thus  it  is  neces¬ 
sary  to  formulate  a  linearized  error  model  which  is  based 
on  the  partial  derivatives  of  the  equations  of  motion  v/ith 
respect  to  all  state  variables.  It  is  this  error  model 
which  is  implemented  in  the  Kalman  Filter.  The  state  vector 
for  the  Kalman  Filter  is  then  defined  as 
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The  nonlinear  system  equations  are  then  rewritten  as 

X 
Y 
Z 


^X-gx  -  2  (wvZ  -  «ozY] 


X  = 


20 


<rtv  l(i)vX  +  wvY  4;  W„Z]  4*  ft  X 

X  J  A  I  h 


-  ZS  Y  -  Ti  *  -  2«“z*  -  “x2> 

R  *'* 

-  uy  tu>xX  4  WyY  4  uzZ]  4  n2Y 


-  JL.  z  -  z  -  2  [wvY  -  «VX] 


20 


“  «z  [wjjX  4.«yY  4  WgZ]  4  Q  Z 


The  extended  Kalman  Filter  equations  are  applied  by  setting 
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The  differential  equation  for  these  error  quantities  can 
then  be  written  in  matrix  form  as 


X  -  P  X 


where  F  was  defined  by  equation  (16) .  It  should  be  noted 

I 

that  although  this  is  an  error  model,  the  system  descrip¬ 
tion  matrix,  F,  the  state  transition  matrix,  ♦  ,  and  the  ob¬ 
servation  matrix,  M,  are  functions  of  the  total  estimated 
states i  The  total  estimated  states  are  determined  by 

numerically  integrating  the  nonlinear  equations  of  motion 

/  *.  £ 

and  subtracting  out  the  estimated  error.  Thus  the  total 

'  ,  ✓ 

states  are  being  "controlled". 

)  This  is  the  fundamental  difference  between  applying  the 
filter  to  a  linear  system  and  to  the  deviations  of  a  non¬ 
linear  system.  N 
Observation  Equations 

'  V'  0 

Observations , of  the  ref entry  vehicle  are  made  every  At 
seconds  by  means  of  a  phased- array  radar.  It  is  now  neces¬ 
sary  to  decide  which  quantities  will  be  treated  as  observ- 

I  < 

ables.  Measurements  are  made  of  the  azimuth,  A;  elevation. 
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E;  range,  R;  find  range-rate,  R  (doppler  velocity)  of  the 

i 

re-entry  vehicle  with  respect  to  the  aircraft  coordinate 

v  *.  • 

system.  Figure  1  shows  the  geometry  and  gives  the  relation¬ 
ship  between  the  radar  and  the  aircraft  coordinate  systems. 

Since  the  filter  is  "being  mechanized  as  an  error  model, 
it  is  necessary  to  treat  errors  in  the  observations . as  the 
measurements .  Thus  the  "measurements"  for  the  Kalman  Filter 
are  actually  differences  between  system-indicated  and  mea¬ 
sured  position  and  range-rate. 

If  the  measurement  is  not  given  directly  in  the  compu- 

a 

tational  coordinates,  it  must  be  properly  transformed 
through  knowledge  of  the  particular  geometry  involved'.  The  . 
transformation  can  either  be  performed  outside  the  Kalman  1 
Filter  or  take  place  in  the  measurement  matrix,  M. 

The  vector  of  observables  was  chosen  to  be 


Z  « 


-  x„ 

C  .  O 

AX 

-  Y 

c  o 

AY 

-  zn 
c  o 

AZ 

•  • 

. 

• 

R  -  R 
— c  sJ 

AR 

(19) 


where  the  subscripts  "c"  and  "o"  refer  to  computed  and  ob¬ 
served  quantities  respectively.  The  measurement  matrix,' 
M,  is  thus  defined  as 


M 


1 

0 

0 

0 


0 

1 

0 

0 


0 

0 

1 

0 


0 

0 

0 


SxR 


0 

0 

0 


■>R 


ZR 


0 

o 

< 

0 

0 


(20) 
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Where  the  three  non- zero  elements  in  the  last  row  are  the 
direction  cosines  betv/cen  the  X,  Y,  Z  tangent-plane  exes 

■?  j  *■ 

*  \ 

and  the  radar  line-of-sight. 


.  Then  / 


Z  «  M  X  +  W 


where  VJ  is'  a  vector  of  white  measurement  noises.',  '  , 


^The  measurement  noise  covariance  matrix,  R,  is  .fUnCtiOn- 

^S  »  '  ,  , 

ally  dependent  on  the  statistics  of  the  sensor  errors  and 

„  _  ,  %  $  „ 

the  orientation  of  the  sensor.  Since  the  \  vector  was 

»  ,/  *  * 

chosen  to-be  the.  th¥ee  position  errors  and  range-rate  error/ 

*  ’■  .  •  ‘  .  i  ' 

it  is  necessary  to  transform  the  noise  errors  of. azimuth, 
elevation^  and  range  into  noise  in  the  three  position 

>  ^  t  c-  - 

/  •'  -  .  ,  \ 

errors .  ^  0  ' 5 

W  tf  f  « 

The  relationship  -between  “the  position  vector  of  the  re¬ 
entry  vehicle  in  radar/ coordinates  is  given  by 


cos  E  cos  A. 


y  b  -  cos  E  sin  A 
a  o 

Z_  sin  E, 


Taking  the  differential  of  equation  (22)  yields 

-R  cos  E  sin  A  -R  sin  E  cos  A  cos  E  sin  A  AA 
AY„  -R  cos  E  cos  A  .  '  R  sin  E  sin  A  -cos  E  sin, A  AE  (23$ 

a  %  f  o 


R  cos  E 


sin  E 


Equation  123)  is  defined  as 


AX  =  A  AV 
■"  “a  ~ 


t 

►  *  ♦ 


fi 
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Figuf#  2  AIRCRAFT  COORDINATE  SYSTEM 
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Now,  we  see  that 


W  =  CE  A  V 

— 1  =A  -  -1 


where  is  the  three  position  components  of  the  measure¬ 
ment  noise  vector,  Vj,  is  noise  in  the  radar  position  mea- 

“  K»  ^ 

surements,  CT  is  the  direction  cosine  matrix  from  aircraft 

•”A  °  v' 

T 

coordinates  to  earth  coordinates ,  and  is  the  direction 
cosine  matrix  from  the  earth  coordinates  to  the  tangent- 
plane  coordinate  system.  The  covariance  matrix  of  the  posi- 
tion  components  of  the  measurement  noise,  denoted  R  ',  be¬ 
comes  .  ‘ 

I  m  Ip  p  •  I  m  -p  m 

R  *  E  ‘  [WA  Wp  -  iCg  A]  R  (c£  (£  A}1  (26) 


where 


r"  =  E  (Vx  vj)  =  0 


0  0 


4  0 


2 

°R 


The  total  covariance  matrix  for  measurement  noise  has 
the  form  , —  — . 


f  •  I 


R11 

R12 

R13 

0 

1 

9 

I 

R21 

1  R22 

R23 

0 

9 

9 

R31 

R32 

R33 

0 

0  0  0 


TABLE  II 
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NOMECLATURE  FOR  KALMAN  FILTER 


_AX  -  Down-range  position  error  of  vehicle 

.  *  >  v' 

AY  -  Cross  range  position  error  of  vehicle 
AZ  -Vertical  position  error  of  vehicle 
A  -  Azimuth  angle  of  vehicle  relative  to  aircraft 
E  -  Elevation  angle  of  vehicle  relative  to  curcraft 
R  -  Range  from  aircraft  to  vehicle 
X  -  Aircraft  longitude 
L  -  Aircraft  latitude 
f  -  Aircraft  heading 

*  0  4 

Y  ~  Aircraft  flight-path  angle 
h  -  Aircraft  altitude 
Rg  -  Radius  of  earth 

JJ 

CA  -  Aircraft- to-earth  transforraation  . 

T  - 

C£  -  Earth-to-tangent-plane  transformation 
CXR#CYR'CZR  ”  Direction  cosines  between  X,  Y,  Z  axis  a 


F  - 

<o 

4  - 


radar  line-of-sight 
System  Description  Matrix 
S,tate  Transition  Matri^ 


M  -  Measurement  Matrix 
K  -  Filter  coefficients  Matrix 
P  -  State  covariance  Matrix 
Q  -  Input  noise  covariance  Matrix 
/R  -  Measurement  noisd  covariance  Matrix 
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Linearization  About  Estimated  Trajecf 


So  far  it  has  been  assumed  that  a  nominal  trajectory  is 
available  for  linearization  purposes.  A  procedure  similar  s 
to  that  suggested  by  Schmidt  (Ref  4)  is  used  to  eliminate  the 

need  for  the,  assumed  trajectory.  As  mentioned  previously, 

■  . 

the  total  states  are  being  controlled.  The  total  estimated 

v  * 

,  states  are  determined  by  numerically  integrating  the  non- - 
linear  equations  of  motion  and  subtracting  out  the  estimated 
error.-  The  control  equation  is 

±(+>  »  ±(-)  -  X  (29) 

A  ",  .  A 

where  3C  contains  the  estimates  of  the  total  states  and  X, 

fr 

the  errors  in  the  states.  Thus,  we  are  always,  linearizing 

«.  *  .  »  '  _  „  r 

i  about  out  estimated  trajectory.  This  could  cause  large 

0  ° 

•  errors^ initially  in  the  linearity  assumptions  since  the  ' 

initial  estimated  trajectory  could  be  way  off.  However, 

*  a  " 5  ■ 

/  *  '  o 

the  estimates  improve  rapidly  and  the  assumptions  become 

*  *  * 

val;Ul.  - 

o  v  *> 

o  o  ' 

Filter  Equations  Simplification  ", 

4  *  *  i  # 

#  *•  -  # 

Not  only  does  this  technique  provide  a  good  "nominal” 
trajectory  to  linearize  about,  but  it  also  provides  a  sim¬ 
plification  of  the  Kalman  Filter  equations.  Equation  (5) 
can  be  written  as 

.  W"  in  k  +  W2n+1  '  2n+l  in  A1  <30) 
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Since  the  total  variables  are  now  being  controlled  in  addi¬ 
tion  to  being  estimated, 


A 

X  =  0 
— n 


Immediately  after  the  measurements  are  made,  the  next  esti¬ 
mate  of  the  system  errors  is  given  by 


— n+1  =  -n+1  — n+1 


The  simplification  eliminates  the  need  to  compute  £n5»n  and 

ML..  ♦  X  .  The  matrices  $  and  M  are,  however,  still 
—n+1  — n— n  — n  —n+1 

required  for  the  calculation  of  Kn+i» 

This  completes  the  necessary  equations  for  implementa¬ 
tion  of  the  Extended  Kalman  Filter.  We  must  determine  the 

A 

initial  values  for  the  estimated "trajectory  and  values 
for  the  initial  state  covariance  matrix  as  well  as  de~ 

'  fine  the  tangent-plane  coordinate  system  which  is  the  com¬ 
putational  frame  for  filter  mechanization.  : 

initial  Estimate  Of  Trajectory 

To  apply  the  Kalman  Filter,  an  initial  estimate  of  the 
state  of  the  nonlinear  system  and  the  covariance  matrix  of 
errors  in  this  estimate  must  be  available.  A  reasonable 
way  of  obtaining  this  is  by  use  of  the  least-squares  fit  to 
a  polynomial.  The  coefficients  of  a  second  order  polynomial 
were  determined  by 
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'  i  '  r  I 

where  the  summations  are  from  1  to  N.  Coefficients  of  Y  and 

s  >  *  | 

Z  were  obtained  similarly.  Note  the  inverted  matrix  is  the 
sane  for  all  three  cases.  The  values  of  X,  Y  and  Z  are  the 
components  of  the  position  vector  from  the  aircraft  to  the 
vehicle  expressed  in  earth  coordinates  by  rotating  the  vec¬ 
tor  through  the  aircraft-to-earth  direction  cosines  C^. 

Thus  the  polynomial  fit  is  applied  to  the  three  earth  com¬ 
ponents  of  the  vehicle  trajectory. 

The  vehicle  is  nominally  tracked  for  four  seconds  before 

*  <• 

the  coefficients  of  the  least-squares  polynomial  fit  are  • 

5 

calculated.  Then,  estimated  position  vectors  of  the  vehicle 
in  earth  coordinates  are  calculated  for  tine  equal  to  zero 
and  tine  equal  to  four  seconds  by 

X(t)  =  £0  *  axt  +  a2t* 


A  A  a  A  o 

Y  (t)  -  bc  +  b±t  + 

A  A  A  A  .  .  2 

Z(t)  s  co  +  c^t  +  c2(t) 

A  ,/a  J  a  j  A  5 

R(t)  =  tfX(t)  +  Y (t) i  +  Z{t)* 


(34) 


These  two  position  vectors  are  used  to  establish  the 

tangent-plane  coordinate  system  and  the  direction  co~ 

T 

sines  from  earth- to-tanqent-plane,  C„  are  calculated.  A 

£i 
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velocity  estimate  at  time  equal  to  four  seconds  is  calcu- 
lated  by 

X(t)  -  a2  +  2  a2t 

•  A  A 

Y (t)  =  b.  +  2  b,t  (35) 

A  -  '  - 

•  A  A 

Z(t)  f  cjL  +  2  c2t 

i  j 

I 

where  these  equations  ^re  the  time  derivatives  of  the  poly- 
nomials  in  equation  (3/4) .  The  components  of.  position  and 

j  .**■  !-r-«  v 

velocity  are  then  rotated  into  the  tang ent-pj. ane  system  and 
become  the  initial  conditions  of  the  estimated  states  fop  ... 
the  start  of  Kalman  filtering. 

Initial  State  Covariance  Matrix 

A  technique  exists  whereby  the  covariance  matrix  for 

/ 

the  estimated  states  can  be  determined  from  the  variances  . 

1  / 

assumed  for  the  radar  system  (Ref  6) .  however,  these  esti¬ 
mates  are  not  cr/itical  to  the  process  so  long  as  they  are 

not  grossly  underestimated.  Studies  show  that  it  is  better 

/ 

to  overestimate  the  error  for  self-correlation  terms  rather 

/ 

than  to  underestimate,  whereas,  it  is  better  to  underesti¬ 
mate  the  crofes-correlated  terms.  Thus,  we  choose  to  set  all 
cross-correlation  terms  equal  po  zero,  and  calculate  the 
diagonal  terms  by 


11 


P22  =  P33  =  (RefE} 


77 


py 

-  read  as  in^ut  data 


P44  =  P55  =  P66 


(36) 


/ 
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where  R  is  the  range  of  the  vehicle  from  the  aircraft,  '  oE  ' 

is  the  nas  value  of  elevation  angle  error  of  the  vehicle, 

*•  • 

and  At  is  the  tracking  tine  for  the  least-squares  fit. 
Elevation  error  was  chosen  because  it  is  generally  larger 
thani  azimuth  error.  This  technique  has  proved  to  estimate 
position  error  about  50  percent  high  and  velocity  error 
about  100  percent  high  v?hen  compared  to  the  fitted  error 
for  the  geometries  and  radar  errors  considered. 

These  initial  guesses  could  use  some  refinement  since 
our  studies  have  shown  the  dynamic  response  o£>  the  filter 
to  be  a  function  of  P_. 


Determination  Of  Tangent-Plane  Coordinate  System 


^  In  the  analysis,  radar  measurements  were  collected 
nominally  for  four  seconds.  This  data  was  used  to  form 
preliminary  least-squares  curve  fits  to  the  trajectory 
for  the  purpose  of  obtaining  initial  position  of  the  ve- 

f  $ 

hide  at  acquisition  and  acquisition  plus  four  seconds, 

0“.' 

as  described  previously.  Denoting  the  position  vectors, 
in  earth  coordinates,  at  tines  zero  and  four  seconds,  as 
^  and  .R^  respectively,  the  product 

-  R  X  R,  i 

'  — o  —l 


R  X  Ri 
— o  -~l 


defines  thfe  unit  vector  which  is  normal  to  the  trajectory 
plane  and  along  the  Y^p  axis  as  shown  in  Figure  3.  The 
product 
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i  XR 
— n  ■< 


N  ' 


^6 


(38) 


defines  the  unit  vector  which  is  down-range  and  along  the 

1  -9 

XTp  axis.  The  unit  vector-  in  the  vertical  direction  is 
simply  '  , 


5o 


N 


=  i 
—V 


(39) 


Thus,  the  tangent-plane  coordinator  systexa,  which  is  the 
computational  frame  for  the  Kalman  Filter, pas  been  estab¬ 
lished.  *  ,  -  - 

Components  of  these  vectors  on  eart if  coordinates  fro**, 

m 

a  direction  cosine  matrix  C£  between  tHe  earth  and  the 
tangent-plane  coordinate  systems, .where 


CT  = 


6X 

i6Y 

X6Z 

■  .. 

nX 

V 

xnZ 

(40) 

vX 

XvY 

XvZ 

'  : 

The  transformation  between  aircraft  and  tangent-plane 


is  simply 


c?  =  c!c? 

-A  —A 


(41) 


Any  inversion  transformation  is  simply  the  transpose 
since  direction  cosine  matrices  are  orthonorraal. 
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Figure  4  Kalman  Filter  Mechanization 
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IV.  SIMULATION 


r  "  , «  ,  1 

A  computer  program, is  implemented  to. evaluate, the  Kalman 

C  •  •<  «  '  •  .  *  i'"  .  ' 

Filter.  An  airborne  radar  platform  is  simulated  tb; provide  . 

>y  .  -  *  %  ^  ‘  s  *  rj  ' 

tracking  data.  A  radar  model, and  an  aircraft  model  are  used 
to  simulate  the- airborne  radar  platform^  .  .Altitude,  velocity , 

.  s  >  ,1  , 

*  \  °  t  ►  *  *1  * 

heading,  latitude,  and  longitude  describe  the  initial  flight 

*  •.'*  „  01  •  , 

conditions  of  the  aircraft.  Azimuth,  elevation,  range,  and  ... 
range-rate  from  the  aircraft  to  the  reference  trajectory  are 


calculated  by. use, of  the -radar 


mo^el . 


Noise  is  added  to  the 


radar  information*  to  corrupt  these  perfect  measurements  v^A 


noise  mod^l  is  used  to  provide  Zero  mean  Gaussian  noise  for 

o  •  >J  ,  ?  •  Jf  f  ,  •  I  •  •> 

°  »  '  .  \ .  .  /  '  *  •-  *  ,  °  > _  '  i  <t 

any  specified  standard  deviation. ^  By  also  specifying  an- 


*  auto- correlation  time  constant/,  it  can  produce  ^exponen- 

‘  '  7 

tialiy  correlated  rtoise*j  (Ref  7}  *  * 

v  l  ‘  i  &  ,  l .  ■"<  i'1  .  '  7 '  ■  •  * 

,  An  Adams- Moulton,  Adams- Bash f dr d  predictor-cgrre&tc$r 

'  *  •  -  /*  •'*  tr/ 

method  is  used  to  integrate  the  non-linear  equations  of 

V  >  ^  #  •  '  '  t,  «.  0 

motion  for  the  reference'  trajectory.  A  Rung.g-Kutta  method 

*  *  i.  •  f.  ’  .  •  .  ,  - 

Is  used  to  integrate  the  -Tahcrent-Plane  Kalman  Filter  tra-  . 

.  »  *  *  ~  \  ' ,  ”*  - 

ject^ry.  The  error  estimates  fx?omsthe  Kalman  filter  model 

c  ®  ‘  <■  * 

are  subtracted  from  the  non- linear  equations  Of  mdtion  to>  • 
r  *  '  •  ,  ■ 

1  give  the  best  estimate  of  the  position,,  velocity',  and  bal- 

r  t a  "  x  •  * 

*4istic  coefficient  of  the  b^llisfeic  missile^ _  .  „  . 

•  >  *  *  f 

The  priifte  element  ift  any  ^intercept  problem  is  the  abil- 

ity  to  accurately  predict  the  position  of  the  missile  at 

■  -  a  o'*  ^ 

some  future  time.  This 'prediction  is  accomplished  by  inte- 

»  •  *  • 

-  grating  the  equations,  of  motidn,1  using  as  initial  conditions 


•  •  «/-’ 


V 


O  i 


c 


Figure  5  Simulation  Model 
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9 

1 

the  non-linear  states  that  are  corrected  by  the  Kalman  fil¬ 
ter  error  estimates.  The  prediction  result  is.  evaluated  by 

c  '  • 

comparing  it  to  the  reference  trajectory  (Figure  8  through 
Figure  19). 


V.  RESULTS 


rv 


/ 


GGC/BE/69-*l£. 


«:  ''  "i 


<£  *  - 

.Four  trajectories  are  used  to  e/valuate  the  Kalman  fil¬ 
ter:  two  aircraft  missile  configurations  in  combination 
with  high  and  low  ballistic  coefficients.  Configuration  A 


was  constructed  so  that  the  vehicle  flew  past  the  aircraft 


(Figure  6) This  configuration  allows  us  to  investigate  the 


effect  of  having  no  velocity  information  about  the  missile 

-  A 


(zero  range-rate)  during  part  of  the  tracking  period.  This 


<r 


MISSILE  GROUND  TRACK 


AIRCRAFT"  GROUND  TRACK 
Figure  6  Aircraft-Missile  Configuration  A 

•«  .  *•  *  1  b  “i  _ _ ^  --  — 


• -  V 


occurs  when; the  distance  Between  the  aircraft  and  the  /missile 
is  at  :.a,  jainimura.  . 

Configuration  9  is . constructed  so  that  the  vehicle  is 

*  • 

always  approaching  the  aircraft  (Figure  7).  This  configura¬ 


tion  allows  us  to  investigate  the  effect'  of,  having  non-zero 

«,  r> 

range-rate  information  for  the  entire  period  of  observation. 

Configuration  A  .  ,  Configuration  3- 


<<$'  -  500’  lb/ft' 


Figures 
8,  9,  10. 


0  *1/750  lb/ft2 


14^15,  16 


^Figiif'es ; 
11,  12,  13 
17,  18,  19, 
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The  position  errors*  Pigures  (8*11*14*17)  show  the  actual 
position  errors  between  the  reference  trajectory  and  the 
estimated  trajectory.  Also*  three  plots  of  position  pre¬ 
diction  error  are  shown  as  prediction  was  started  with  the 

«  i 

— - >•  i - * 

MISSILE  GROUND  TRACK  AIRCRAFT 

GROUND  TRACK 

r  __  &  > 

Figure  7  Aircraft-Missile  Configuration  B 

information  available  after  ten  seconds*  twenty  seconds*  and 

\ 

thirty  seconds  of  processing  data  through  the.  Kalman  filter. 

One  would  expect  better  prediction  results  after  more  data 

o 

has  been  processed.  However*  by  inspection  of  position 

\ 

prediction  errors  for  Configuration  A*  Figure  (8)  and  Figure 
(14),  this  is  not  always  the  case.  In  order  to  explain  the 

l  . 

, effect  of  increased  position  prediction  error  after  more  data 

i’  \ 

has  been  processed*  the  velocity. errors  and  the  estimated  , 

7  * 

*  *  >  , 

ballistic  coefficient  must  be  examined  at  the  start  of  pre- 

»  .  \  *  '  -  ' 
diction.  In  either  the  high  or  low  ballistic  coefficient 

case*  the  velocity  error  decreases  at  first*  then  increases* 

and  finally  decreases  again.  During  the  period  of  the  first 

decrease*  the  missile  is  above  the  atmosphere  and  an  incor- 

0 

4  rect  estimated  ballistic  coefficient  has  no  effect  on  the. 
trajectory.  As  the  missile  enters^the  atmosphere  with,  an 
incorrectly  estimated  ballistic  coefficient*  the  velocity 
error  starts  to  increase  due  to  the  functional  relationship 

‘  f  ^ 
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between  the  velocity  of  the  missile  and  its  ballistic  co¬ 
efficient.  Also#  during  the  period  of  increasing  velocity 
error#  the  range-rate  is  approaching  zero  as  the  range  from 
the  aircraft  to  the  missile  approaches  a  minimum.  As  more 
data  is  processed  through  the  Kalman  filter#  the  estimated 
value  of  the  ballistic  coefficient  nears  its  actual  value  • 
and  the  velocity  error  decreases. 

For  aircraft-missile  Configuration  B,  there  is  an  ex¬ 
pected  asynpotic  decrease  in  the  velocity  error,  due  to  the 

availability  of  non-zero  rancc-rate  during  the  entire  track- 

* 

ing  period.  However  prediction  errors  have  not  significantly 

improved  over  Configuration  A  because  during  prediction 

the  value  of  the  estimated  ballistic  coefficient  is 

incorrect.  The  prediction  errors  do  not  increase  as 

rapidly  at  the  start  of  prediction  as  in  Configuration  A# 

but  stij.1  do  increase.  ,  The  delay  in  the  error  build-up 

is  due  to-  the  small  velocity  error  at  the  start  of  prediction. 

However  as  prediction  continues  an  incorrectly1  estimated 

ballistic  coefficient  causes  the  velocity  error  to  increase 

rapidly  thereby  increasing  the  position  errors  also.  One 

may  conclude  that  no  matter  how  accurate  the  position  and 

velocity  of  the  missile  is  known  at  the  start  of  prediction, 

the  prime  clement  in  the  prediction  problem  is  the  ‘ballistic 

coefficient.  In  order  to  arrive  at  any  firm  conclusions  a 

parametric  study  must  be  made;  such  as#  accuracy  as  a 

(  1 

function  of  tracking- tine#  tracking  geometry#  and  a  priori 
information# 
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Figure  13  Estimator!  Ballistic 'Coefficient 
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figure  19  Estimated  Ballistic  Coefficient 
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The  following  IBM  360  Scientific  Subroutines  were 


also  used: 


i 


'  Subroutine  MPRD 

LOC  ,  j  , 

.  .  MTRA  «  ’*  j  - 

* 

.  .  '  *  MCPY  •  *  ‘ 

•  .  ,<  i  ' 

The  following  7094  Fortran  IV  Function  was  also  used : 

'  i  '  .  *  .1  .  ,  * 

'DDT  »  Detemihant  Evaluating  Function 
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(C(  001 1  *-T 
1CI006) (STEP 


1 1 ( C 1 002 I »  T  F 

> 


SIBFTCEXEC.  , 

COMMON  C 1999 1 
EQUIVALENCE 
1  « 

1  CALL  ZERO 

2  CALL  INRUT 
LSTEP-STfP 

9  CALL  INI TAL 
A  CALL  OUPTl 

5  CALL  ACTION 
CALL  OUTPUT 
IF<T<LT<TFI  60  TO  S' 

CALL  RESET 

60  TO  1 1*2  •  St-4.  S  *61  *LSTEP  ,  >» 

6  STOP 
EN*\ 

SIBFTC  ZERO.  OECK  ..  ' 

C  f 

C  SUBROUTINE  ZERO  SETS  INDICATORS  AND' CONSTANTS 
C 


I* 


/' 


tl 

2. 

T 


SUBROUTINE'  ZERO 
'COMMON  CI999) ^ 
REAL  MU 
EQUIVALENCE 


\  D0‘  1  1*1*999  ■ 

l  cm,— o.o 

norNdm*o  ’ 

NOOUT-O  ' 

$TEP.*2<0  ' 
RE-20926428.0 
MU-1.40775  Eli  - 
VVJE-7.2722E-9 
W1E2ZVI£«W1E  • 

,  F1A-1.0  *  •  ‘ 

'  F25-1.0-  ,  ■  ‘ 

(  -F36-1.0  *  » 

N  RETURN  *  . 

'  end  •  •  *  • 

SIBF'TC  INITL* 

,  SUBROUTINE  INITAL 
CALL  ATMOSI 
’  CALL  TRAJMI  • 

CALL  PLANE I 

call  noise i  .»  ; 

CALL  MATCH 
CALL  KALMAl  ' 

CALL  PREDTI  . 

CALL  COMPAI 

RETURN 

END 

*  *  . 

SIBFTC  ACTIO. 

SUBROUTINE  ACT  10$ 
COMMON  CL999I 
DIMENSION  PTIMEI4) 
INTEGER  P.KOUNT 
.  :  EQUIVALENCE 
,  1  %  • 

.  1  CALL  missle 
•CALL  PLANE 
•CALL  NOISE 
CALL  RADAR 
3  CAlL  KALMAN 
°  call  compar 


I C 1 490 1 1  NORN  DM  )  . ( C ( 499  > •  NOl.I  ST »  .  <  C  <  500 1 .  NOOUT  I  . 
ICIOllI.RE  I • ICI012I *MU  ) • (C(006I *STEP  I. 
(C( 013) tWIE  )«<C(014I»W1E2 
(CI222I *fl4  I  *10  230) *F25 


I 

)  * (C (23?  ) *F36 


I 


/ 


I C <  00 1 1  * T  I  •  (C  (008  )  t'TTSKF  ). 

* C <  016)  (PJCOUNT I  *  ICI017 1  *Pt  IME  J 


•  r 
V' 


IFIPT IME(PKOUNT I .LE.O.OL  RETURN  i 
IRIT.LT.PTIME IPKOUNT ) I  RETURN 


KT 


CALL  PREDIC 
RETURN 
END 


GGC/EE/69-15 


SIBETC  MLS  St.  DECK  « 

C  SUBROUTINE  MI  SSL E  GEMdiATESTHE  REFERENCE  TRAJECTORY  >IN 
C  EARTH  ANO  TANGENT  FLAME  COORDINATES 
C 


SUBROUTINE  MJSSCE 
‘COMMON  CI999I  / 

EQUIVALENCE  (C(lOl)  »XEM  ltfC<102ltYEM 

1  (01041  tVXEM  ItlCIlOSltVYEM 

2  »(CI 120) tXTM  l.(C(121l»YTK 

5  (CI123I tVXTH  |t(C(124ltVYTM- 

4  100311 tCETIl  |t(C<034)tCET12 

»  (O032-)  *CET21  I t(C<035) tCET22 

6  *  (CI033I »CET31  )t(C(036I*CET32 

7<C(OU)tR£  >»IC(l06)<H  *  I t (CI109)  *V  | 

CALL  TRAJM  '  *  ■ 

, H-S0RT(XEM»XEM+YEH»YE»H-ZEM*ZEM>-QE 
V*SQRT ( VXEM»VXEM+y Y  EM*VYEK+VZEM*VZEM  J  • 

CALL  ATMOS (H*RH0t6AMA) 

0«0.5*RHO»V»V  .  •> 

' XTM®CET11*XEM+CET12*YEM+CET1 ;*ZEM 
YTM-CET21 *X£M*CET22  *YEM+C£T2  3*ZEM  - 
ZTH»CET31*XEM+CET32*Y£M+CET33»2EM 
VXTM=CETn*VX£M+C£T12*VY£M+C£T13*VZEM 
VYTM»C£T21*VXEK+C£T22*VYEM+CET23*VZ£M 
VZTM*C£T3l«VX£K+C£T32*VYEM+C£T33*VZ£H’ 

RETURN  A  •  ,  ‘ 

END-  .  '  ..  . 


) »(C I 103} tZEM  I» 
it  (0106! tVZEM  It 
It(C(L22lt2TM  I. 

1 t (CC125) tVZTM  It 
I t  (0037)  tCET13  It 
lt4C(038UCET2l  |» 
>ttC(039ltCET?3  lt‘ 
1 1 (Cl  1101  tO  -  I  .. 
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siqftc  trajm.  deck  /  •  \  j  . 

c  '  ,  «  -  '  i 

C  INTEGRATION  ROUTINE  FOR  TM*  REFERENCE  TRAJECTORY 

C  ADAMS-BASMFORTH  -  AOAMS-MOULTON  PREOlCtOR-CORRECltlft  WITH  RUNGE-XUTTA 


*  sumovtine  trajmi 

,  COMMON  CI093I 

OdUSLE  PRECISION*  -< 

'  DIMENSION  D<t>5}.lft6V»l*yf6l.YD<6}  * 

EQUIVALENCE  4CI003I.H  t.tCIOOlt.X  ,  hICllOlliY 
i  *  ICUlll.YD  I 

.  DATA  M/6V  - ^V" 

K-0  ..  1  .  " 

*2-0  ./  .  .  •  * 

DO  10  I-l.M 

10  Wtl»ll-D6LEIY<!}1  „  ; 

CALL  DENT 

DO  1  1-1*0  '  „ 

1  Dll  *5I-YD(  I  >  '' 

RETURN  •  v  „ 

ENTRY  TRAJM  .  »  ” 

40“  XC-X  \  t  - 

IF  IX.NE.O)  IF  JL-J2  J  50*00*110 

•  !  x'p**C  .  '  •  •  ,  • 

00,45  I-l.M,  r  1 

45  *  .  • 

50  y  Kl-4-K  ;  r  •’  .  '  ;*  • 

00  70 

,00  60  JAri.f,  /  V  •  , 

40  OI|*Ji^Dir«J«H  /  '  '  S.--T 

V<I.2)->t*DM,41  ,  y ! 

WIl.lt"WF1.11+.5DO*V(I.2l  ;f.  : 

TO  Yin-SNGVTW(I.ln  •  ,  ‘  .  “w 

x«xcf«Mr  :?’,/>  '•  \-V' 

CALL  D5RT  ■- 


DO  2  I-J.6  *  r  ,,  , 

,  2  Dl|*5l-YDII1  '  -ft->  . 

DO  69  I-l.N  ,  „  '  'V*‘ 

tf(t*5l-H«D(l*51 

.  WCl*lt-Vt!.l}*.5D0»(W(I.3l-Vtl«2ll  -»  • 

•  oa  Ym*sN<a.iWii.i»i  \, 

*  CALL  DERT 
A  .  •  DO  3  1-1.6 

,  3  DII.6I-VDII)  ‘  /  »•  '  „ 

0  .  DO  00  1*1 *N  ' 

MI!.4|-H*D(I.5>  *" 

Ml 1  *1  l-VII.ll+VC  |  .4«-.4D0*Wt  1 .31 
•  00  YC I l-SNGLIWI  1*11) 

X-XO*H  “ 

CALL  DERT  -  '  -  .* 

‘  DO  4  1-1.6  *, 

4  Dl 1 .51-VDI 1 1  -  i  .  o  , 

DO  100  I-l.M  ,,  "•  »  , 

VII.ll-V<I.li-W<I.41+.166666£666&666667*tW{I.2f+2.D0*tN(1.3l*V(I«4 
'  -11»^H*DII.5I1  "  ’  -  >  ‘ 

100  Yni-Sr«GL(Wll',l;i 
■  «S  *  K-K-.1  -  N.  , 

A  »  .  Kl-K*  '  ::>* 

*  /  CALL  OERT  -  -  ,  /  *. 

OO  5  1-1.6 

5  O<I*5t-Y0(I|  •  .  .  I 

RETURN  .  - 

110  DO  130  I-l.M  »  . 

W(I.2t*Vn.ll 
DO.  120  J-1.4 
120  0(I.Jt-0<l.J*ll 

MI  1  .it  -VI I  iil*V41466666666666667D-l*H*(55.*Dt  1  «4l-$9.*Dt  I  iiUil.H 
1 1 l.21-9.*DI I • 1 > 1 
130  VI  M-SNGLIVI I  .31 1 

X-XC.H  ’  ' 

CALL  DERT 
,00  6  1-1*6 

60M«S)-YDII|  .  • 


V«yJ  ' 


i 

# 


4 


r  - 
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Do  140  I-l.M  -*• - : - f - - 

W I  »1>*VI  I ,2!+*6166666666666*667D-l#H>t9.« 
l»3)*OU,2M  i 

1 4ft  Vm«SM6L(W(I»ltt  ,  / 

CALL  DERP  '  / 


L»19«*D(1  •*)-§.  *DI1 


4>0  7  1*1,6 
7ftfI,51*YtHII 


RETURN 

END 


1 


'V 


SIBFTC  DERP.  PTCIC  -- r  "\  , 

t  ‘ 

C  SUBROUTINE  OERP  PROVIDES  THE  DERIVATIVE  LIST  FOR  THE  INTEGRATION 

C  ROUTINE  FOR  THE  PREDICTION  SUBROUTINE  -  TANGENT fPLAk£ 

C  .  '  '  "  ■■  I  \  \  . , 

SUBROUTINE  DERP  *  *  \  5 

COMMON  Cl  999)  1  —  \ 

REALM!  ‘  .  .  *  "  \\> 

EQUIVALENCE  .  ICC9X7I.X  l,(Cf9««)*Y  7,(CI969>ti  *  I, 

1  '  ICI99DI »VX  *»<CJ991)«W  ),(CI99?),V&  • 

2  '  „  .1  ‘  ICI021I.VX  ).(Cf,022)*WY  ),IC<023> ,M*\  I* 

,  -  3ICT993I. ALPHA  I •  (CX.012I *MU  |,IC(01II.RE  I,(C(01AT.H1E2  I, 

(  -4  *  1  .  (Cl 9941. XD  I »ICt995ISY0.  )»(C4996)«2D  ,  \  I, 

U  *  \  .(Ct  9971,  VXD  l»ICI99»l,VYD  -  !i*C<999l,’VZD  \»/ 

R*SQRT  ( Jt*X+Y*Y*2*2l .  ‘  .  .  '  * .  \ 

V*SQRnvX«VX+VY*VY-,V2*V2»  \  5  \  ,  f 

6*NU/(R«*3>  .  . 

“H-R-RE  V  ;  \l 

CALL  ATMOS (H*RH0»GAMA)  A 

D*0.$«RHO  '  *V*ALPHA  a  X 

SUH*VX*X+*Y«Y+M«A  •  .> , 

-  xo*vx  f  A  • 

.  ,  YO*VY  ,  *  «  v  »  '•  r  ,.r  *  ■  . 

2o*vz  ,  y .  \ 

VXD=~G*X-O«VX-2.0*<WY»VZ-WZ»VY)HXX*SyM+X*WIE2  •  , 

VY0«-G*Y-0*VY-2.0^CWZ*VX-VX»VZJ-VY*SUM+Y*WIE2  0  - 

.  V2D*-G*2-D*V2-2.0*TVX»VY-WY*VXI-irj»SUH+2*Vi£^  , 

■  ,  RETURN  ’  '  0  ■  I  .  i  .  ,  ^  ’ 

■  1  £N0  ’  ‘  ,  *  (  !' 


'*  j  5 


v  — - 


/ 
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SIBFTC  PLAN!*  DECK 
C 

C  SUBROUTINE  PLANE  -  AIRCRAFT  HOOEL  -  GENERATES  AIRCRAFT  POSITION  AMO 
C  AIRCRAFT-TO-EARTH  DIRECTION  COSINES 


C 

C 

c 


c 

c 

c 


c 

c 

.*c 


SUBROUTINE  PLANE I 
COMMON  0999) 

REAL  LAT.LOMG.LATRtLONGR 
EQUIVALENCE  (C(126)*LAT 

1  ICC  129 J tHEAD 

2  ICC  1311 »XEP 

3  (C(134)*VXEP 

4 

5 


1  *101271  (LONG 
t*ICC1301*VP 
I »(Ctl32l*YEP 
1»CCI135)*WEP 
(00411  *CAE11  1  »(O044)»CA£12 
(CIC421  *CAE21  )*  10045  )*CAE22 


)»(C(046)*CAE32 
I  *10001 1  »T 


6  (Cl C43) *CA£31 

7  (OOlllfRE 
DATA  CDTR/1.7453293E-2/ 
LATR-LAT*CDTR 
L0N5P.-LCKG*CDTR 
H£AD!t=H£AD*CDTR 
5L0NG=S1NILC:;GR) 
CL0f.&*C0SIL0.;S11 

SLAT'S) NIL ATS) 

CLAT=COSILAT:.j 

SMEAD'SIKIME/.DR) 

CH£AD=COS(HEADR) 


CALCULATE  INITIAL  AIRCRAFT-TO-EARTH  DIRECTION  COSINES 

CAE 1 1=-SHEA0*  SLCNG-SLAT * CHE AD 5 C LONG 

CAE21=SHEAD*CLON6-SLAT*CHEAD*5LONG 

CAE31'CLAT*CM£AD 

CAE12=CH£AD*SL0NG-SLAT*SMEAD*CL0«G 

CAE22'-CHEA0«CL0XG-SLAT«SH£A0*SL0HG 

CAE32*CLAT*SH£AD 

CAE13=CLAT*CL0NG 

CAE23=CLAT*SLONG 

CAE 33 'SLAT 

R'REAHP 

XO=CAE13*R 

YO=CA£23«R 

ZO'CAE33*R 

XEP=XO 

VEP'YO 

ZEP'ZO 

VXEP=CAEI1«VP 
VYEP'CAE21*VP 
VZEP*CAE31*VP 
RETURN 
ENTRY  PLANE 

CALCULATE  NEW  AIRCRAFT  POSITION 

IF  (VP.EQ.O.O)  RETURN 

XE°=XC*VXEP*T 

YEP*YO+VYEP*T 

ZEP'ZC*VZ£P*7 

P2=X£P«X£P*Y£P»YEP 

P=S0RT(P2» 

R*S0RTIP2*Z£P«Z£P> 

HP=R-R£ 

UPDATE  AIRCRAFT-TO-EARTH  DIRECTION  COSINES 

E11*-YEP/P 
E21*XEP/P 
E31*6.0 * 

E13*XEP/R 

E23=YEP/R 

E33=Z£P/R 

f 12*-£?1*E33 

F22*E11«E33 

E32'P/R 


I  •  10128)  »HP  1 
I »  (0137)  *GAMMA  ) 
)*(C(133)*ZEP  ) 

>•(0136)  »VZEP  1 
)»(C(047)»CAE13  ) 
) • (CI04E ) *CAE23  ) 
>»(C(049).CAE33  ) 
I 


51 
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V£«£11*VXEP«-£21*VY£P*£31*VZ£P 

VA=E12«VX£P*E22»VV£P*E32*VZ£P 

VR=*£13*VX£P+E23*VYEP+E33*VZ£P 

VH*SORT I V£«V£*VM«VM I 

SH£AO*V£/VH 

CH£AO=VH/VH 

GAHMA=ATAM2 ( VR  «  VH I 

CAE11=E11*  SHEAD+E 1 2  *CHC AD 

CAE21-E21*SHEAD+E22«CK£AO 

CAE31 *£31*SHEAD+E32  «CHEAD 

CAE12=-E11«CHEAD+E12*SHEAD 

CAE22=-E2 1 «CK£ AD+E22*SH£AD 

CAE32=-E31«CHEA0+E32*SH£AD 

CAE13=E13 

CAE23=E23 

CAE33=£33 

RETUXK 

END 


*  v**  M>  ** 
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SIBFTC  RADAR. 
C 


SUBROUTINE  RADAR  GENERATES  RADAR  MEASUREMENT  DATA 


SUBROUTINE  RADAR 
COMMbN  CI999I 
DATA  CRTD/57. 295779/ 
EQUIVALENCE  (CIlOllyXEK  t 
IC( 1041 »VXEM  I 

ICC131I#XEP  » 

<CI134>«VXEP  I 

10041) fCAEll  'I 

(CI042I.CAE21  I 

(CI043) *CAE31  I 

7<CC0W).£PSAZ  > . ICI077I .EPSEL  » 

6(0  070) »AZ  ).<C(OiO).£L  I 

9(0*040) »AZD  I < (Cl  050) »ELD  I 

XaX£K-X£P 
Y*Y£K-Y EP 
Z*Z£H-2£P  ' 

VX=VX£I'-VX£P 
VY=VYEM  -VYEP 
VZ=VZ£)'.-VZEP 

XA=CAEn*X-fCA£21*Y+CAE31*Z 
YA=CAE12*X-»CAE22*Y+CA£32*2 
ZA=CA£13*X+CA£23*Y+CAE33*Z 
A2=ATA.*i2(— YA»XAI+EPSAZ 
XYR=SCi?.T  I  XA«XA+ YA*Y  A I 
EL‘ATAfl2(ZAtXYi:)4£*>S£L 
ft- SORT  I  X«X+Y«Y+Z*Z  | 

RA'R+EPSRA 

RR= I ( X«VX+Y*VY+Z *VZ  J /R I +EPSRR 

AZD=AZ*CRTD 

ELD»EL*CRTD 

RETURN 

END 


*IC(102)»Y£M  )»(C<103) 
ylCIlOSI.VYEM  )«(C(106) 
#(C(132)»Y£P  ) *  (0133) 
#(C(135)*VYEP  ) » {£(136) 
»<C(044)»CA£12  ) * (0047) 
»(C(045) tCAE22  »» 10048) 
*(004$)  *CA£32  )» ICTD49I 
»(C(087>»EPSRA  )«(C(097) 
•  (0090)  »RA  .  >t(CUOO> 


•ZEM  I 
»VZEM  | 
•ZEP  I 
»VZEP  I 
•CAE13  I 
»CAE23  I 
»CAE33  I 
•EPSRR  I 
•RR  I 
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SI8FTC  I 6U£S.  DECK 

SUBROUTINE  IGUESI 
COMMON  Cl 999) 

DIMENSION  AI6)»AXC3)*AYI3|'*..Z(3).BX(3)*8YI3).BZI3) 


EQUIVALENCE 

ICI03D.CET11 

)  *  1C 1034 ) 

•CET12 

)  * ICI037 ) *CET 13 

)• 

1 

ICI032) *CET21 

)  *  1C  1035 ) 

•CE122 

) • CCI036) *CET23 

)• 

2 

ICI033) *CET31 

)  •  ICI036) 

•  CET  32 

)  * ICI039) *CET33 

)• 

3ICI013) *WI E 

) • 1C  1 021 ) *WX 

) • 1C  1022 ) 

•  MY 

)• ICI0Z3) *HZ 

1* 

4 

ICI041) » CAE  11 

) *  1C  1044 ) 

.CAE12 

) * IC 1047 ) *CAE13 

)* 

5 

ICI042) *CAE21 

) *10  045) 

.CAE22 

)  • IC 1048 ) tCA£23 

)• 

6 

ICI043) *CAE31 

) *  1C  1046) 

.CAE32 

)  •  IC 1049 ) *CAE33 

)• 

7 

(Cl 141 ) *XTP 

) *  1C  1 142 ) 

»  YT? 

) • ICI 143) *ZTP 

>* 

B 

ICI 144) »VXTP 

>. 1C  1145) 

•  VY 1 P 

) • ICI146) *VZTP 

It 

9ICI0C9) »TK 

) • ICI 0C1 ) *T 

) • ICI OOB ) 

*TTS£F 

>  • 

1 1C  1070  *AZ 

),(C(CftC).EL 

) •(CIOSCI 

*RA 

) .  ICICll > .RE 

I* 

2 

ICI 101) »XEK 

)• 1C  1102) 

*  Y  hX 

)  •  iti  io3)  t?~y. 

)  » 

3 

ICI 104) tVAEM 

i.ioiot) 

.vrEK 

)  *  I  Cl 106 ) t Vi  EM 

I  • 

4ICI1C7I.3ETA 

) • ICC  140) *£B£TA 

) • IC 1 169 ) 

•S1GEL 

) • (0350) .SluB 

)• 

SIC  1401) *PP11 

)  * ICI 403) *PP22 

) • 1C  1406 ) 

*PP33 

) * ICI410) *PP44 

i  • 

6(0415)  «PPS6 

)  ■  ICI 421 1 tPrCC 

) *ICI428I 

«  PP  7  V 

)• 

7(01281**1? 

)  *  ICI 120) »XTP 

)*tctm> 

»YTX 

)• ICI 122) *ZTK 

) » 

8 

ICI 123) » VXT * 

) • IC 1 1 24 ) 

•  VYT  X 

).ICI12S).VZTM 

1 

C 

C  INITIALIZE  THE  ROUTINE 
C 

TO=T 

DO  1  1=1*2 
BX(I)=0.0 

BY (I) =0.0  ' 

1  EZU)=C.C 
DO  2  1=1*6 

2  All  1=0.0 
XOA=XEM 
YOA=Y£H 
20A=ZEM 
RETURN 

C 

"  ENTRY  IGUESS 
C 

C  COMPUTE  POSITION  IN  EARTH  COORDINATES  FRO*  RADAR  OBSERVATIONS 
C 

C0SEL=C0S(ELl 
XA=RA*C0SEL*C0S I AZ ) 

YA=-RA»C05EL*SI N I AZ I 
ZA=RA«SIN(EL)+RE+HP 
X=CAE11*XA*CAE12»YA+CAE13»ZA 
Y=CAE2i»XA4CAE22*YA+CAE23*ZA 
Z=CAE31*XA+CAE32*YA+CAE33*ZA 
C 

c  LOAD  MATRICES  FOR  LEAST  SOUARES  FIT 
C 

T2=T*T 

T3=T2*T 

Al 1 )=A( 1 )  +  l  .0 

AI2)=AI2)4T 

AI3J=A(3)+T2 

A(5)=AI5>M3 

AI6)»AI6)*T3*T 

BXt 1 1=BXI 1 

6XI2)*BX12)+X*T 

BX13)=BXC3)*X*T2 

8YI1>*BYI1>+Y 

BVI2I«8YI2)+Y*T 

BYI3)=BYI3)*Y*T2 

BZI1>«BZI1>+Z 

BZI2>«B2I2)+Z«T 

BZl3j=B2Cil+ztT2 

IF  I T.LT. I TTSKF-0.0005) )  RETURN 

AI4)*A(3) 

C 

C  COMPUTE  COEFFICIENTS  OF  POLYNOMIALS  FOR  LEAST  SOUARES  FIT 
C 

CALL  S1NVIA.3.1.0E-5.IER) 


54 
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CaU  HPfilHA.BX. AX, 3, 3,1.0.11  r 

CALL  KPP.0IA.8Y, AY. 3. 3. 1,0. II 
CALL  MPRO(A,8Z.AZ.3.3.1.0.1> 

C 

C  COMPUTE  ESTIMATED  POSITION  AND  VELOCITY  AT  TIKE  T 
C 

Xl*AXIll*AX<2l»T+AXI3l*T2 
Y1*AYI 1 J*AYI2I*T  »AYI3|*T2 
21*AZI1I*AZ(21*T+AZI31*T2 
VX1=AXI2I+2,C»AX(3I*T 
VYl*AYI2l+2«0*AY(3l*T 
VZ1*AZI2»*2.0*AZI*I*T 
C 

C  .  COMPUTE  ESTIMATED  POSITION  AT  TIME  TO  . 

C 

X0=( ( AXC  31 »  TO  >+AX(2 1 1 *T0+AXI 1 ) 

VO*IIAYI3)*TO)+AYI2)*»*0+AYll» 

Z0-IIAZI3»*T0)+AZI2)  ,-TO+AZm 
C 

C  ESTALLISH  TANGENT  PLANE  COORDINATE  SYSTEM  AND  COMPUTE  DIRECTION 
C  COSINES  FOR  EARTH-TO-TAf.CENT  PLANE  COORDINATE  TRANSFORMATION 
C 

C1=Y0' Z1-Y1*Z0 

C2=Z0*X1-X0‘Z1 

C3=X0*Yi-XliY0 

D=SQRTIC1*C1+C2*C2+C3*C31 

CET21=C1/D 

C£T22=C2/D 

CE?23*C3/D 

C1*C£T22*Z0-YG*C£T23 

C2»CET23*X0-Z0*C£T21  , 

C3“CET21*Y0-X0*C£T22 
D«=SQRT  CC1*C1*C2*C2*C3*C3 I 
C£T11=C1/D 
CET12*C2/D 
*  CET13*C3/0 

D-SORT I X0*X0+ YO* Y0*Z0*Z0 I 
CET31=X0/D 
CET32=Y0/D 
CET33-Z0/D 
C 

c  COMPUTE  COMPONENTS  OF  EARTH  ROTATION  IN  TANGENT  PLANE 
C 

WX«CETI3*VIE 

t#Y«=CET23*WIE 

MZ*CET33«WIE  S 

C 

C  COMPUTE  INITIAL  ESTIMATE  OF  POSITION  AND  VELOCITY  FOR  KALMAN  FILTER 
C 

XTP»CET11*X1+CET12«Y1+CET13«ZI 

YTP«CET21*X1*CET22*Y1+CETZ3*Z1 

2TP*CET31*X1+CET3Z*Y1+CET33*Z1 

VXTP=CET11*VX1+CET12*VY1+CET13*VZ1 

VYTP=CET21*VX1+C£T22*VY1+CET23*VZ1 

VZTP=CET3:«VX1+CET32*VYUCET33*VZ1 

C 

C  COMPUTE  DIFFERENCE  BETWEEN  ACTUAL  AND  ESTIMATED  VALUES 
C  OF  POSITION  AND  VELOCITY 
C 

DX0*X0A-X0 
DY0=Y0A-Y0 
OZOxZOA-ZO 
DXl«XEM-Xl 
DY1«YEM-Y1 
DZ  WEK-Z1 
PVXl»VAfM-VXl 
6VYlxVYEM-Wl 
DVZ1*VZEM-VZ1 
DBETA*8ETA-EBETA 
C 

C  COMPUTE  INITIAL  VALUES  FOR  STATE  COVARIANCE  MATRIX 
C 

SIGR=S!G£L*RA 


r»  nr» 
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SIGR2*SIGR*SIGR 

$IGV=SIGR/T 

S1GV2*SIGV*S1GV 

IF(SIGB.EO.O.O)  SIGB=100.0 

PP11=SIGR2 

PP22=SIGR2 

PP33=SICR2 

PP44-SIGV2 

PP55-SIGV2 

PP66=SIGV2 

PP77=1.0/ISIGB*SIGB> 

OUTPUT  CONDITIONS  TOR  START  OF  KALKAN  FILTERING 

WRITE (6 >600)  AX*AY.AZtXOA>XO.DXO>YOA>YO#DYO>ZOA.ZO>DZO>T>XEM,Xl> 
1DX1>SIGR>YEM>Y1,DYI.SIGR.?F\.Z1.BZ1>SJGK>VXE:'.>Va1.DVX1,SIGV  > 
2VY£M>VY1>DVY1>S!GV  >VZEK>VZ1 >DVZ1.S1GV  >3ETA>EBETA> DoETAtSIGB 
60C  FORMAT C 18HILEAST  SO’JARES  FJT/lHA>62X>lH2/7H  X  =  1PE14.7>5H  +  > 

1E14.7.7H  T  ♦  >E14.7.?H  T/lHA.62X>lH2/7h  Y  =  >EI4.7>SH  +  • 

2E14.7.7H  T  ♦  >£14.7>2K  T/lHA >62X>lH2/7H  Z  =  .E14.7.5H  ♦  > 

3EJ4.7.7H  T  +  >E14.7>2H  T///1KA. 14X,fcHACTUAL. 11X >9HEST lKATEDt 

46X> 10KDIFF£REfJCE> 10X>  SHSIGVA/ 17K0T IKE  =  0  SECONDS/ /H/./O  =>3£1£.7 

5/7HCYC  =>3E!8.7/7h0Z0  =>3Eie.7/7HAT IFt  =>CPFS.*>£h  SECONDS/ 

67HAX1  *t 1P4E18«7/7HCY1  * • 4E18. 7/7H021  = >4Ei8.7/7riAVXl  =, 

74E18.7/7H0VY1  =>4E1B.7/7K0V <■  =>4ElB.7/7HAb£IA  =>4E18.7) 

XTM=CETll*XEM+C£Tl2*Y£M+CET13«2EM 
VTM=CET21*XEK*CET22*YEM+CET23*Z£M 
*  ZTM=CET31*XEfI+C£T32*YEM.+CET33*ZEM 

VXTK=CET 1 1*VXEM+CET 12*VYEM+CET 1 3*VZEM 

VYTM=CET21*VXEM*CET22*VYEH+CET23*VZEM 

VZTH=CET  31*VXEM-»CET  32*VYEM+CET33*V2£M 

CALL  COKPAR 

CALL  OUTPUT  V 

TK*T 

RETURN 

END 


I 
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c 

X 

(7X1) 

STATE  VECTOR  (TANGENT  PLANE) 

c 

z 

(4X1) 

VECTOR  OF  1BSERVABLES 

c 

K 

(7X4) 

FILTER  GA)N  MATRIX 

c 

R 

(4X4) 

MEASUREMENT  NOISE  COVARIANCE  MATRIX 

c 

PE 

(7X7) 

FILTER  ESTIMATION  COVARIANCE  MATRIX 

c 

PP 

(7X7) 

FILTER  PREDICTION  COVARIANCE  MATRIX 

c 

PHI 

(7X7 ) 

STATE  TRANSITION  KATRIX 

c 

PHIT 

(7X7) 

TRANSPOSE  JOF  STATE  TRANSITION  MATRIX 

c 

F 

(7X7) 

SYSTEM  DESCRIPTION  MATRIX 

c 

DXEST 

(7X1) 

VECTOR  OF  OPTIMAL  ESTIMATION  OF  ERRORS  IN  STATES 

c 

CET 

(3X3) 

DIRECTION  COSINES  ' 1 EARTh-TO-TARGRT 1 

c 

CAE 

(3X3) 

DIRECTION  COSINES  (AIRPLANE-10  EARTH) 

c 

CAT 

(3X3) 

DIRECTION  COSINES  (AIRPLANE-TO  TARGET) 

c 

PAD77 

(7X7) 

.  SCRATCH  PAD 

c 

PAD74 

(7X4) 

SCRATCH  PAD 

SUBROUTINE  KALMA1 
COMMON  Cl 999  I  1 

INTEGER  PGCNt  , 

REAL  K(7,4I.K44,)'45,H46  . 


DIMENSION  X(7) .Z(4).CVC3).Pf ' 7 .7) .PP( 26 1 ,PH1(7«7 1 .PHIT C7.7 1 ,Rl7l • 
1FI7.7).DXE;ST!7J.CE7 (3 .3 ) .CAE ( 3.3) .CAT (3.3 ) ,PAD77( 7,7 1 ,PAD74( 7 ,4 ) , 
2Pf7'4(7,4),PAD47(/  ,7  )  ,G1 10J  ,  A( 7  )  »‘a' ( 3) 


ECUIVALEr.CE 
1ICC070J.A2 
2101411, X 
3(0  201),  F 
4(C('CC3).D7 
5(C< 361 I «M44 
6(0149), EBETA 
7(0 166)  «SIGAZ 
8(C( 137), GAMMA 
9(0148), K 


(C4 C31 ) ,C£T 
) , (C(C£0) ,EL 
),CC(161),DXEST 
) , (C( 251) ,PK1 
1,10010)  ,DT2 
I » (Ci  362) »K43 
) , (C( 128) ,HP 
) , (C( 169) tSIGEL 
1 . (C( 0151 .EPS 
) , (C( 149) ,V  ) 


),(C 1041), CAE 
)  ,  (C  (090  ,RA 
) , (C( 157 ) ,Z 
), 10301), PE 
)  ,  (C(011 ) «RE 
) ,  (C  (303)  ,.".46 
),  (CUSS)  .PGCNT 
)  .  (C(17C) »SIGRA 
> .  (C( 138) ,SEPR 


I,  (0091),  CAT 
) ,  (0100)  »RR 
),(C(173),K 
1 »  (C  ( 351 )  *R 
) ,  (0130)  «VP 
! ,  (0172)  »D 
) , (C ( 401) ,PP 
),  (0171). SIGRR 
)  ,  (C( 139) »SEPV 


,  (0955)  .SEPR1  1,40  936), SEPV1  ) 


DT2=DT*0T/2«C 
SIGR2=S1GRR*S IGRR 
EPS2=EPS*EPS 


X(7)=1.0/EBETA 
RETURN 
ENTRY  KALMAN 


C 

C  COMPUTE  THE  SYSTEM  DESCRIPTION  MATRIX  -  F 
C 

CALL  SDH 

C 

C  COMPUTE  STATE  TRANSITION  MATRIX  -  PHI  AND  PHIT 
C 

CALL  MPRD(F,F .PAD77 ,7, 7, 0,0, 7) 

DO  11  1=1.7 
DC  10  J=1 ,7 

10  PHICI,J)=F(I,J)«DT+PAD77(I,J)*DT2 


11  PHI <1,11=1 »0+PHI (1,1) 

CALL  KTRAlPHI ,PHIT,7,7,0) 


C 

C  UPDATE  FILTER  ESTIMATION  COVARIANCE  KATRIX  -  PE 

c 

CALL  MPRDIPHI ,PP.PAD77.7.7.0»1»7) 

CALL  MPRDIPAD7 7, PH I T,P£,7«7»0»0»7) 

DO  IS  1=1.7 

15  PE ( I , I ) *PE ( I , I )+R< 1 1 
D=DET ( PE.7 ) 

IF(D.EQ.O.O)  KR1TE( 6 ,600 ) 

600  FORMAT ( 1HA, 1 OX, 1 Oh* «***«***« , 10X.35HSTATE  COVARIANCE  MATRIX  IS  SIN 
1GULAR  . 10X. 1 OH* ********* ) 


C 

C 

C 


UPDATE  MEASUREMENT  MATRIX  -  M 


SA=SIN(A2) 
CA=COS(AZ) 
SE  =  S1 N<  EL ) 
CE=COS(EL) 
CRA1=CE»CA 
CRA2=-CE*SA 


J 


a  aa  a  a  a  a  aa 


GGC/EE/69-15 


CRA3-SE 

CALL  MPRDICET  tCAEtCAT«3»3»0»0»3l 
M44=CAT(1 # 1 I «CRA1*CAT( 1 »Z I «CRA2+CATI1 . 3 » *CRA3 
M45=CAT C  2 . 1 1 *CRA1+CAT (2*2) «CRA2+CAT 1 2 . 3 1 *CRA3 
H46-C AT J 3. 1 » »CRA1+CAH 3 #2 J »CR A24CAT ( 3 t 3 I »CRA3 

CALCULATE  THE  MEASUREMENT  NOISE  COVARIANCE  MATRIX  -  C 

RSIGA*RA«SIGAZ 
R  S  TGE  *  RA*S I GEL 

Will =CRA2 *RSI 6A-SE»CA*RS I GEaCRA 1*S I GRA 
Nt2 J=-CRAX*RSI6A*SE*SA*RSIQe+CRA2*SIGRA 
M|3>=CE«RSIGE*SE*S1GSA 
CALL  MPRDICAT  »tf»CVt3»3«0»0»l ) 

COMPUTE  FILTER  GAIN  MATRIX  -  K 

DO  20  1-1(7 

20  A(l)=K44*PE(4(Il+K45ipEI5(l)+K46*PElfc(Il 
0(1)-PE<1|1)KVU!>CV(1I 

0(21 =P£(  1 (2)4CVtlJ*CVI2l 
QI31=PE(2(2)+CV(2)*CVI21 
QI4l=P£ll(31+CVl 1I--CVI31 
0(5I-PE(2#3)-!-CV(2I*CV(3) 

QI61=PE(3.3)*CV(31*CV(3) 

0(7|=A( 1 1 
Q(E)=A(2) 

Q(9 ) =A( 3 ) 

OI10)=M44*A(41+M45*A(5HK46*A(6) 

CALL  S1NV  (0(4d«OE~CS( TER) 

DO  22  1*1(7 
DO  21  J=l(3 

21  PAD74(I(JI=PE( I(J) 

22  PAD741 1 (41=A( 1 1 

CALL  MPRDIPAD74(0(RP74(7(4(0(1 (4! 

CALL  MTRA(PAD74.PA047s7(4(0l 
CALL  MPRD(PP74(PAD47(PAD77(7(4t0(0t7l 
PAD74(l(ll=PA074(l(l)+EPS 
PAD74I2(2)=PAD7412.2)*EPS 
PAD74 ( 3( 3 I *PAD74 C 3 ( 3 l+EPS 
PAD74I4(4)=PAD74I4(41+M44*EPS 
PAD74(5(41=PAD74(5(4I+M45*EPS 
.  PAD74I6(4I=PAD74(6(4J+M46*EPS 
CALL  MPRD(PA074(0(X(7(4(0(1(4I 

UPDATE  FILTER  PREDICTION  COVARIANCE  MATRIX 

DO  30  1=1(6 

30  PPI I l=Q( I 1*EPS2 
DO  31  1=7(10 

PPI I I=Q( I 1*M44*EPS2 
J=I+4 

PPI J1=QI I 1*M45*EPS2 
KK=I*9 

31  PPI KK1 *Qt I )*M45*EP52 
PPI 10)*PP( 10 )*M44 
PPI 15)=PPI 14 1 «M45 
PPI14)=PP(14)»M44 
PP(21)*PP(19I*M46 
PPI201=PP( 19|*M45 
PPI191*PPI19)»H44 
DO  32  1*22(28 

32  PPI 1 1*0.0 
KK*1 

00  33  J=l(7 
DO  33  1=1(J 

f>P(k^)*Mfo:>4>£(I(J)-PAD77l!iJ) 

33  KK*KX+1 

SEPR*SQRT1PP(l!*PP(l!+PP(3l*PPt31*PP(61*PP(6| 1 
SEPR1*SQRTCPPI1)4PP(31*PP(6)J 

SEPV=SORT(PP(101*PPI101+PPI151»PP115)4PP(21)«PP(21)1 
SEPV1*S0RT I PP 1 10 l+PP 1 15 ) +PP 1 2 1 1 1 


58 
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INTEGRATE  THE  EQUATIONS  OF  MOTION 


CALL  TRAJK 


.JO 


CALCULATE  OPTIMUM  ESTIMATE  OF  ERRORS  IN  STATES 
*EHP=R£4HP 

2  til *X 1 1 1 -M4 4*RA-CAT 1 1 • 3 1 »REHP 
2I2I=X(2I-MA5*RA-CAT(2.3)«REHP 

2131  *XI3I-K46»RA-CATI  3.3  J*REHP  * 

2  C  4 1  =M4  4*X  ( 4  J  +M*.  5*X  1 5 1  -rl'.46*X  C  fc I - VP* ( CRA 1  *COS  ( GAMMA )  +S£*S  1 N  ( GAMMA  >  I 
I-RR 

CALL  MPRDIK.2.DXEST.7.4.0.0.1I 


UPDATE  STATES 

DO  AO  1=1.7 
AO  JCIl 1  —  X < 1 1-OXEST 111 

H-SORT (X(l|*X(lI4X(2i*X(2l+X(3)*X(3l J-RE 

V=S0RT(X(CJ»XIA|+XI5»*XI5J4X(6I*XI6)I 

ECETA=1.0/XI7t 

RETURN 

END 
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SIBFTC  SDH.  DECK 
C 

C  SUBROUTINE  SOM  COHPUTE^HE  SYSTEM  DESCRIPTION  MATRIX  FOR  KALMAN  FILTER 

SUBROUTINE  SOM  ' 

COMMON  C(«l*)  . 

REAL  MUL  , 

•  DIMENSION  FI7.7J 


EQUIVALENCE 

ICI011) »RE 

1 .  (C (012 ) »KU 

) . (C (201 ) »F 

)* 

1 

ICI141) *X 

).(CI142).Y 

) »  <C (143) .2  v 

)* 

2 

(Cl  144 J  » VX 

).(C(l45)*VY  * 

) . (Cl  146) »VZ 

). 

3-, 

(Cl 147). ALPHA 

).(C(013).W1E 

) .,(C(014 ) »rtIE2 

). 

4 

(CI02D.WX 

) .(CI022 I >WY 

) . (C (023) »WZ 

) 

RESORT ( X*X+Y»Y+Z*Z I 

V=S0RTCVX*VX+VY*VY4V2*V2»  . 

G=MU/R**3  ’  , 

% ,  ? 

CALL  ^TMOS ( Hi RHO  *  PftHO ) 

D*0.5*RHO*V*ALPHA  ,  % 

T1*3.0*G/IR*R) 

T2  *  D6PRHO/  ( Kt(0*R )  •  , 

eT3=D/(V*V) 

T4“~D/ALPHA  , 

TX«=T1*X-T2*VX 

TX3-T3«VX 

F I 4* 1 I =-6+TX*X-WX«HX4Wl E2 
FI4.2)*  TX*Y-WX*UY 

FI4.3)*  TX*Z-VX*WZ  * 

F(4.4)«-D-TX3*VX 

FI4.5I*  -TX3*VY42.0«WZ 

FI4.S)*  *-TX3*VZ“2  •  0*NY  , 

FI4.7)«T4*VX 

TY*T1*Y-T2«VY 

TY3-T3*VY 

f(5.1|«  TY*X-WX*WY 
FI 5 .2 »*-64TV*Y-NY«NY4WI E2 
F(5»  31*  TY*Z-WY*WZ 

FI5.4J*  -TY3*VX-2.0*VZ 

F(5.5j*-D-TY3*VY 
FC5.SI*  -TY3'VZ42.0*WX 
FI5.7)*T4«VY 
TZ*T1*Z-T2*VZ 
TZ3*T3 *VZ 

Fl6»l»*  TZ*X-WX*¥Z 

F(6*2)*  TZ*Y-MY»WZ 

F|6t3l*-€4TZ*Z-NZ*«Z4«IE2 

F 16*4)*  -TZ3*VX42.0*WY 

FI6.5)*  -TZ3*VY-2.0*VfX 

F(6»G)*—DtTZ3*VZ 

F(6»7)*T4*VZ 

RETURN 

END 
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/  •  .  • 

SI8FTC  TRAJK.  t>eC< 
jC 

C  INTEGRATION  ROUTINE  F OR  KALMAN  FILTER  TRAJECTORY 

c  double  Precision  runge-kutta 

C  ' 

subroutine  trajk 

*  COMMON  C(999l 

EQUIVALENCE  (0  009) »T  |,(C(003)aH  I  * 

1  »  CC( 141 I «X  I • (C( 151 ) »XO  ) 

DIMENSION  XN(6)tX(4)fXD<6) 

DOUBLE  PRECISION  XN»Cl(6)tC2(6l *C3 ( 6  V 

DO  1  1*1.6 

1  XN(I)=DE.'lE(X(I>) 

TC=T  ’ 

•  CALL  DERX 

•  DO  2  1*1.6 

'ClUl-Hi'XOUl  .  * 

a-  XNI I  )EXfi(  1  ).«5DG*>C1  <1 1 

2  xm.*s.*:GL«>.Nmi 
T=7C+*5'i! 

CALL  OiKR 
DO  3  1  =  1.?.  ' 

C?m=H*Xylir 

XN(n*XNm  +  .5£)3i(C2U)-CHI|» 

<•  3  Xin=SN'L(XMmi 

Call  derl  ”  . 

a  00  4  1*1,6 

v  C3cn=H-xpui  o 

.  kN<n*xN(i)+c3m-.5Do*C2m 

4  X(I)*SNGL(XK(ni 
T*TC,H 

CALL  DERX  *  -  ^ 

DO  5  1*1.6 

XN< I l-XNI 1 1-C3I I >♦. 16666666666666667* (Cl 1 1 )+2.D0*(C2 < 1 ) +C3 ( I 1 1 
1*H»X?(1>) 

5  XC I l*SNGL(XN( III 
RETURN 

END 


SIBFTC  DERX.  ,  DECK  ,, 

C  - 

C  SUBROUTINE  DERK  PROVIDES  THE  DERIVATIVE  LIST  FOR  THE  INTERGRATION 
C  ■  ROUTINE  IN  KALMAN  FILTER  -  TANGENT  PLANE  4  7 

C 

•  SUBROUTINE  OERK 


Common  ci99»» 

REAL  MU 

0 

. 

equivalence 

(Cl  1411 ,X 

I.ICM42I.Y 

I.ICI143I .2 

), 

1 

(CI144I.VX 

1 ,  (<(145 I  ,VY 

I.ICI146I.VZ 

). 

”2 

(Cl 1471 .ALPHA 

)  •  (ClOll 1 .RE 

). 

3 

(CI021) ,HX 

1 , (C (022 1 «WY 

) . (C 10231 ,W2 

It 

4  4 

(Cl  0121 .MU 

I.ICI013) .VIE 

l.(C;014l,WIE2 

)• 

5  . 

(0151), XD 

l.(CU52).YD 

I  > (C ( 153 ) .ZD 

), 

6  1 

(C ( 154) .VXD 

1  »(C ( 155  J ,VYD 

) , (C 1 156) »VZD 

) 

R«SORTCX»r+Y*Y+2#2) 

V-SCRT ( VX6VX.VY* VY*VZ*VZ  J 

G»MU/(R**3) 

H-R-RE 

CALL  ATM0SIH.RH0.6AMAI 
0«0.5*RH0  *VMLPHA 
SUM-WX»X+VY«Y-.WZ»Z 
XD-VX  ' 

.  YD-VY 
2D*VZ 

VXD'«-G*X-D»VX-2.0»CWY»VZ-«Z»VYI-WX»SUM+X*HIE2 
VYD--G* Y-D* V Y-2 . 0* I WZ«VX-WX*VZ I -WY*SUM*Y*WI E2 
VZD— G*Z-D*VZ-2.0*<WX*VY-VY»VX)-WZ»SUM+Z*VIE2 
RETURN 
END 
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JIBFTC  PftEDC.  DECK 
C 

C  ,  SUBROUT  IKE  PREDIC  GENERATES  PREDICTED  VALUES  OF  POSITION 
C  FROM  THE  PRESENT  TIME  -  T  -  TO  THE  FINAL  TIKE  -TF 
C 

,  SUBROUTINE  predii 
COMMON  CI999I 

COMMON/PREDC/ AA ( 500  » 4 J . AD ( 400 . A I .AC ( 300 . 41 
INTEGER  PKOUNT  . 

DIMENSION  XK(7I #XP(7) .COUNT (3 J 

EQUIVALENCE  (C 1 001 J .TIKE  I.ICI002I.TF  |.IC(003|.DT  '  ) 

1  C C f 9C2 1 « COUNT  i*(C<9eS>.T  >»(C(9£Gr.HP  I 

2  ICIIAII.XK  I » ICC967 1  #XP  I » (C 1016 1  .PKOUNT  I 

IFIHP.LT. DTI  HP=DT 

PKOUNT =1  ^ 

RETURN 
ENTRY  PREDIC 
J*1 

T*TIKE 
DO  1  1  =  1.7 

1  XPIll-^III 
CALL  TRAJ? I 

GO  TO  13.5.7) .PKOUNT 

2  J=J+i  /  / 

CALL  TitAJP  / 

60  TO  13.5.7) .PKOUKT 

C 

c  COMPUTE  PREDICTION  -A- 

C  1  ; 

3  AAIJ.lJ-T 
DO  4  K*2.4 

4  AAU.K)'XP(K-l) 

.60  TO  9 

C 

c  COMPUTE  PREDICTION  -B- 

5  ABIJ»1I*T 
DO  6  K«2.4 

4  AB( J»K)*XP(K-1 I 
GO  TO  9 
C 

C  COMPUTE  PREDICTION  -C- 

C 

7  ACI J*1)*T 
DO  •  K*2»4 

•  ACI  J»Jlfc)*XPIK-l ) 

*  IFIT.'tT.TF)  GO  TO  2  . 

COUNT (PKOUNT )« J  / 

PKOUNT*PKOUNT+1 

RETURN  “  / 

END 
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SI8FTC  TRAJP.  DECK 

INTEGRATION  ROUTINE  FOR  THE  FREOICTION  SUBROUTINE 

ADAMS— BASHFORTH  -  ADAMS- HOUL TON  PREDICTOR-CORRECTOR  WITH  RUNG E-CUT T A 

SUBROUTINE  TRAJPI 
COMMON  CI9991 
DOUBLE  PRECISION  W 
DIMENSION  D(6.5|«W(6.5I .YC61.YDI6J 

EQUIVALENCE  (CI9B6I.H  ItlC(985ltX  )t(C(987l*Y  It 

I  CCC994) tYD  I 

DATA  M/6/ 

K«0 
K2*0 

DO  10  1*1. M 
10  Wd . 1 )=DELE( Y( III 
CALL  OEP.P 
DO  1  1*1.6 

1  D< 1 .5 l*YD( 1 1 
RETURN 
ENTRY  TRAJP 

AO  XC*X 

IF  IK. RE. 01  IF  IK-2)  50.50.110 
XP*XC 

DO  45  1=1. M 
A5  Wd»5l*'«d*l| 

50  K1-4-K 

DO  70  I*1»M 
DO  60  J=X1 .4 
60  dd.JI-Dd.J+ll 
Wd »2|*H*D1I.A1 
Vdtll*Wd*ll+.5D0*Wd.2t 
70  Y (  I )*SKGL (fed .11  I 
X«XC*.5*H 
CALL  DERP 
DO  2  1*1.6 

2  Oil *5l*YDI 1 1 
DO  BO  I*1.M 

HI  I »31*H«Dl 1.51 

°  W(I«1I=W( 1 . 1 I+.5DO* (Wf I »3)-WC 1 .21 1 

BO  Y(  I  )*SNGL(t>(  I  til  I 

CALL  DERP 
DO  3  1*1.6 

3  D(I.51=YD(I| 

DO  90  1*1 .M 
WI!.4|*H«D(I*5! 

W(I.l)=Wd *1 )+W( I >A )-.5D0*W( I .31 
90  Y(I 1-SNGLIWI 1 .1!  I 

X*XC4H 
CALL  DERP 
DO  A  »!*1.6 

4  DlI.5l*YDd) 

DO  100  1*1 .M 

Wd.ll=Wd.ll-Vd.4l+.16666666666666667*IWd.21+2.D0*IWd.3l*WII»4 
lll+H*0d»5)  I 
100  Ydl-SN5LIWd.il) 
x=r.*i 
K1*K 

CALL  DERP  • 

DO  5  1-1*6 

5  Dfl.5l*YDdl 
RETURN 

110  DO  130  1*1 *H 
Vd.2l-Wd.il 
DO  120  J-1.4 
120  OdtJl*Dd.J*ll 

W( I t3l*V< ! .2)+.4l666666666666667D-l»H»<55.*DI 1 »41-59.*D(l i3>+37.*0 
ld.2l-9.*Dd.lll 
130  Yd  l*SNGLfW(  1 .31 1 
X*XC*H 
CALL  DERP 
DO  6  1*1.6 

6  DJI .51 *YD1 1 1 
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DO  140  1*1 »M 

W( 1 ,1 )«W( I .2  )«■  .41666666666666667D-1*H*I 9.»OI 1 .5) +19.«D< 1 ,4 )-5.*D< 1 
1 *3>+D( 1*2)) 

140  Y( I )=SNGLIWI  1  <11 1 
CALL  DERr 
DO  7  1*1.6 
7  DII»5)*YDII) 

RETURN 

END 

* 


SIBFTC  DERT.  DECK  /  \ 

C  SUBROUTINE  DERT  PROVIDES  THE  DERIVATIVE  LIST  FOR  THE  INTEGRATION 

C  ROUTINE  FOR  THE  REFERENCE  TRAJECTORY  -  EARTH  COORDINATES 

SUBROUTINE  DERT  \  / 

COMMON  CI999)  \/ 

REAL  MU 


EQUIVALENCE 

ICtlOll.X  ' 

).IC(102).Y 

) • ICI 103) *Z 

). 

1 

ICI 104) *VX 

) . ICI 105) .VY 

) » ICI 106) *VZ 

). 

2 

(CM07)  »8ETA 

).ICI011).RE 

)  . 

3 

ICI021) .WX 

) » 1C  1022 ) *WY 

) , ICI023) .WZ 

). 

4 

(CI012I.MU 

>.10013). HIE 

) *  ICI014) »VIE2 

)> 

5 

ICIllll.XD 

) .ICI 112) . YD 

) • ICI 113) .ZD 

). 

6 

ICI 114) .VXD 

) » ICI 115) .VYD 

)» ICI 116) .VZD 

) 

R*SORT  CX*X*Y*Y*Z*Z I 
V*SORT I VX«VX+VY*VY.VZ*VZ » 
G=MU/IR»*3) 

M*R-RE 

CALL  ATMOS I H.RHO. GAMA) 

D*0.5*RHO»V/BETA 

XD=VX 

YD*VY 

2D*VZ 

VXD=-G*X-D*VX+2 . 0*VI E* VY+X*W I E2 

VYD*-G«Y-D*VY-2. 0*V I E«VX*Y»*  I E2 

VZD=-G«Z-D»VZ 

RETURN 

END 
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SlBFTC  COMPR.  DECK 
C 

C  SUBROUTINE  COMPAR  COMPUTES  THE  DIFFERENCE  BETWEEN  THE  ACTUAL  VALUES 
C  OF  POSITION  AND  VELOCITY  AND  THE  ESTIMATES  AMD  PREDICTED  VALUES 
C 

SUBROUTINE  COMPA1 
COMMON  CI999I 

COMKON/PREOC/AA ( 500 .4 ) . AB 1 400 . 4 ) , AC ( ?  C  0 . 4 ) 

C0MM0N/CALC0M/TTI700) »DR(700) *PDR( 70C! • OVJ 700) «P0V(700) *£BI700l .1 « 
1TAI5C0I tPDRAI  500),*J»TB(400) »PDSE>(400  > i> .TCI  300) »PBRCI 300) *K 
INTEGER  PKOUNT  .  > 

EQUIVALENCE 
I (Ct 107 1 .BETA 
2 

3(0140) .EfcETA 

4 

5 (C 1030) (D3ETA 
6 
7 

• 

OTH=O.C*C5 
1=0 
'  J=0 

K=0 
L=0 
RETURN 

ENTRY  CO.'IPAR 
C 

C  COMPUTE  ERRORS  IN  ESTIMATION 
C 

DELX*EXTM-XTM 
DELY*EVTM-YTH 
DELZ=EZTM-ZTM 
DELVX=EVXTM-VXTM 
DELVY«EVYTM-VYTM 
DELVZ*£VZTM-VZTH 

OELR*SORT<OELX*OF.LX+DELY*OELY+OELZ*DHLZ> 
OELV=SQRTIOELVX«DELVX+DELVY«OELVY+DELVZ<DELVZ) 

QBETA*EBETA>BETA 

c 

c  load  arrays  for  plotting 

c 

1*1+1 
TTII)=T 
OR  11 )*OELR 
DVI I l*OELV  * 

ESI  I )*E8ETA 
GO  TO  (7.5.3.1 ) .PKOUNT 
C 

c  COMPUTE  ERRORS  IN  PREDICTION  -C- 

C 

1  L*L+1  V 

0IFF«T-ACIL.1I  ^ 

IFIABSCDIFFI.GT.DTHI  GO  TO  2  ,  ( 

OELPRC*SORT I  I ACIL.2 )-XTM)«*2+( ACIL .3 )- YT(I) »*2+ (AC I L»4)-ZTM)*Vj) 

C  LOAD  ARRAYS  FOP.  PLOTTING 

TCILI'T 

PDRC(L)*DELPRC 

2  I-F (OIFF.GT .0.0)  GO  TO  1 
C 

C  COMPUTE  ERRORS  IN  PREDICTION  -B- 

C 

_3  K*K+1 

DIFF*T-ABCK.1» 

IF  I ABS(DIFF) .GT.DTH)  GO  TO  4 

DELPRB«S0RT((AB(K.2)-XTM)*»2+(AB(K.3)-YTK)«*2+(AB(K.4)~ZTH)«*2) 

C  LOAD  ARRAYS  FOR  PLOTTING 

TB(K) *T 

PDRB(KI*DELPRB 
4  IFIDIFF.GT.O.O)  GO  TO  3 
C 

C  COMPUTE  ERRORS  IN  PREDICTION  »-A- 

C  • 


(Cl  120)  .XTM  ).(C(I;'i)  .Yfy.  )  .  (C  ( 122 )  #ZTM  ) 
) » (C( 123 ) . VXTM  ).(C(1 --i.V/Ti;  ) . (C ( 125 ) ,VZTM  ) 

(C(lAI).EXTM  • » (C( i V2  * i EYTK  ) » (C ( 143) ,EZTM  | 
) .  (C(  1441  .EVXT.N  ).(C'j.  :  .l-Tl::  ) .  ( C  ( 146)  .EVZTM  ) 
(C  ( 024 )  iDELX  )  .(C(C2-^:  .v-LY  )  .  (C  (026 )  ,DELZ  ) 
I  «  (C(  027 1  .OELVX  ).(C(C  LVr  )  .  (C  (029)  .DcLVZ  ) 

(C(  118)  (DELR  ).(CC;/i  )  .  (C  (016)  .PKOUNT ) 

(C(  976)  .DELFRA)  » ( CC?_"  ’  »  (C  (973 )  .DELPRC) 

(C(133).SEPR  ),(C4i;  - ?v'  ).(C(001)»T  ) 
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5  j* J+l 

DIFF=T-AA(J,1 J 

!F(ABS<DIFF).GT.DTH)  GO  TO  6 

D£LPRA=SQRTt ( AA( J*2 l-XTM|**2+( AAI J»3)-YTM)«*2+lAAl J*41-ZTM1*»2) 
C  LOAD  ARRAYS  FOR  PLOTTING 
TA< J)*T 

PDRA( JI=D£LPRA 

6  IF(OIFF.GT.O.O)  00  TO  5 

7  RETURN 
ENO 
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St&FTC  NOISE.  DECK 

SUBROUTINE  NOISE  GENERATES  GAUSSIAN  NOISE 

SUBROUTINE  N01SEI 
COMMON  CI999I 
INTEGER  RNDMNOI 5 I . IX( 5) 

EQUIVALENCE  I C I A90 » tNORNDM ) . I C ( 491 ) . RNDMNO ) • ( C 1 003 ) »DT 

IF(NORNDM.EQ.O)  RETURN 
DO  1  I>  I  tNOP.NDM 
J. RNDMNO ( 1 1 

IF ( C  C  J+2  > .LE . 0 .0 »  C I J+2 » >0.0000001 
Cl J+3l>2.7le2818*»(-DT/C<J+7JI 
Cl J+4)rCC J+l  J+SQRT  ( 1 .0-C ( J+31 *C  ( J+3 )  J 
IXI*=CIJ> 

CALL  RANDUIIXI .IY.VJ 
IXIII-IY 

1  C(J+6)*CIJ+1)*V 
RETURN 
ENTRY  NOISE 

IF(ho;:nd:;.eo. o i  p.etukk 
DO  2  I=l.NORND.*1 
j>RND:t;jou» 

IXI-IXI I > 

SUM-0.0 
DO  3  X'l t 12 
CALL  RANDUUXltlY.V) 

IXI-IY 
3  SUH*SUM+V 
X« SUM-6.0 
ixm-ixi 
CIJ+5|*CIJ+6| 

2  CCJ+6l«=C(J+4i»X+CIJ+3)«CIJ+5» 

RETURN 
END 
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IBFTC  RANDU.  DEC* 


SUBROUTINE  RANDU 
PURPOSE 

COMPUTES  UNIFORMLY  DISTRIBUTED  RANDOM  REAL  NUMBERS  BETWEEN 
0  AND  1.0  AND  RANDOM  INTEGERS  BETWEEN  ZERO  AND 
2**31.  EACH  ENTRY  USES  AS  INPUT  AN  INTEGER  RANDOM  NUMBER 
AND  PRODUCES  A  NEW  INTEGER  AND  REAL  RANDOM  NUMBER. 

USAGE 

CALL  RANDU ( IX.IY.YFLI 
DESCRIPTION  OF  PARAMETERS 


RAN0U0C0 ' 
RANDU0C1 j 
•RANDUOGZ . 
RANDL0G3 . 
RANDUC C4 " 
RANDU0C5 ' 
RANDUGC6 ' 
RANDU007 
RANDUOCS ' 
RANDUGC9 
RANDU010 
RANDU011 
RAND0G12 
RAHDU013 
RANDUC 14 
RANDUC 15 
RANDU016  ’ 


IX  -  FOR  THE  FIRST  ENTRY  THIS  MUST  CONTAIN  ANY  ODD  INTEGER  RANDU016 
NUMBER  WITH  NINE  OR  LESS  DIGITS.  AFTER  THE  FIRST  ENTRY .RANDUC 17 
•  IX  SHOULD  BE  THE  PREVIOUS  VALUE  OF  IY  COMPUTED  BY  THIS  RANDL'0 1 8 
SUBROUTINE.  RANDU019 

IY  -  A  RESULTANT  INTEGER  RANDOM  NUMBER  REQUIRED  FOR  THE  NEXTRANDU02C 
ENTRY  TO  THIS  SUBROUTINE.  THE  RANGE  OF  THIS  NUMBER  IS  RANDU021 
BETWEEN  ZERO  AND  2**31  RANDU022 

YFL-  THE  RESULTANT  UNIFORMLY  DISTRIBUTED*  FLOATING  POINT.  ’  RANDUC 2 3 
RANDOM  NUMBER  IN  THE  RANGu  0  TO  1.0  RANDUG2L 


REMARKS 

THIS  SUBROUTINE  IS  SPECIFIC  TO  SYSTEM/360 
THIS  SUBROUTINE  WILL  PRODUCE  2**29  TERMS 
BEFORE  REPEATING 

SUBROUTINES  AND  FUNCTION  SUBPROGRAMS  REQUIRED 
NONE 

METHOD 

POWER  RESIDUE  METHOO  DISCUSSED  IN  IBM  MANUAL  C20-B011* 
RANDOM  NUMBER  GENERATION.  AND  TESTING 


SUBROUTINE  RANDU! IX.IY.YFLI 
IY=»1X*262147 

IF ( I Y.LT. 0 J  IY3! I Y+ 34359738367 1*1 
YFLMY 

YFL»YFL*.29103383046E-10 

RETURN 

END 


RANDU022 
RANDUC 2 3 
RANDUG2L 
RANDU025 
RANDU026 
RANDU027 
RANDU028 
RANDU029 
RANDU030 
RANDU031 
RAN DUO 32 
RANDU033 
RAN  DUO  34 
RANDU035 
RANDU036 
RAN DUO 3 7  ' 
.RANDU038 
RANDU039 
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♦I8FTC  ATMOS.  DECK 
C 

C  SUBROUTINE  ATMOS  PROVIDES  AIF  DENSITY  AND  RATE-OF-CHANGE  OF  AIR  DENSITY  AS 

C  A  FUNCTION  OF  ALTITUDE.  AIR  DENSITY  IS  ACCURATE  TO  WITHIN  2.0  PER-CENT 

C  OVER  AN  ALTITUDE  RANGE  OF  -10.000  FEET  TO  +2.000 .000  FEET  AND  TO  WITHIN 

C  0.2  PER-CENT  IN  THE  RANGE  -1.000  FEET  TO  40.000  FEET.  •-  INTERPOLATION  IS 

C  LINEAR.  TABLE  ENTRIES  ARE  FROM  THE  19S9  ARDC  MODEL  ATMOSPHERE. 


C 


c 

-10.000 

FEET 

1.0150 

E-01 

LBS/CU.FT 

c 

-5.000 

FEET 

8.6310 

E-02 

LBS/CU.FT 

c 

-1.000 

FEET 

7.6738 

E-02 

LBS/CU.FT 

c 

SEA  LCVE 

L 

7.6475 

E-02 

LBS/CU.FT 

c 

1.000 

FEET 

7.4262 

E-02 

LBS/CU.FT 

c 

2.000 

FEET 

7.2099 

E-02 

LBS/CU.FT 

c 

4.000 

FEET 

6.7918 

E-02 

LBS/CU.FT 

c 

6.0G0 

FEET 

6.3926 

E-02 

LBS/CU.FT 

c 

6.000 

FEET 

6.0116 

E-02 

LBS/CU.FT 

c 

10.000 

FEET 

5. 6463 

E-02 

LBS/CU.FT 

c 

12.000 

FEET 

5.3022 

E-02 

LBS/CU.FT 

c 

14.000 

FEET 

4.9725 

E-02 

LBS/CU.FT 

c 

16.000 

FEET 

4.6589 

C  -02 

LBS/CU.FT 

c 

16.000 

FEET 

4.3006 

E-02 

LBS/CU.FT 

c 

20.000 

FEET 

4.0773 

E-02 

LBS/CU.FT 

c 

22.000 

FEEt 

3.80&3 

E-02 

LBS/CU.FT 

c 

24.000 

FEET 

3.5531 

E-02 

LBS/CU.FT 

c 

26.000 

FEET 

3.311? 

E-02 

LBS/CU.FT 

c 

26.000 

FEET 

3.0S23 

E-02 

LBS/CU.FT 

c 

30.000 

FEET 

2.8657 

E-02 

LBS/CU.FT 

c 

32.000 

FEET 

2.6609 

E-02 

LBS/CU.FT 

c 

34^000 

FEET 

2.4676 

E-02 

LBS/CU.FT 

c 

36.000 

FEET 

2.2852 

E-02 

LBS/CU.FT 

c 

36.000 

FEET 

2.0794 

E-02 

LBS/CU.FT 

c 

40.000 

FEET 

1.8895 

E-02 

LBS/CU.FT 

c 

45.000 

FEET 

1.4873 

E-02 

LBS/CU.FT 

c 

50.000 

FEET 

1.1709 

E-02 

LBS/CU.FT 

c 

55.000 

FEET 

9.2185 

E-03 

LBS/CU.FT 

c 

60.000 

FEET 

7.2588 

E-03 

LBS/CU.FT 

c 

65.000 

FEET 

5.7164 

E-03 

LBS/CU.FT 

•c 

70.000  sHFEET 

4.5022 

E-03 

LBS/CU.FT 

c 

75.000 

FEET 

3.5463 

E-03 

LBS/CU.FT 

c 

60.000 

FEET 

2.7937 

E-03 

LBS/CU.FT 

c 

65.000 

FEET 

2.1784 

E-03 

LBS/CU.FT 

c 

90.000 

FEET 

1.6901 

E-03 

LBS/CU.FT 

c 

95.000 

FEET 

1.3182 

E-03 

LBS/CU.FT 

c 

100.000 

FEET 

1.0332 

E-03 

LBS/CU.FT 

c 

110.000 

FEET 

6.4392 

E-04 

LBS/CU.FT 

c 

120.000 

FEET 

4.0851 

E-04 

LBS/CU.FT 

c 

130.000 

FEET 

2.6349 

E-04 

LBS/CU.FT 

c 

140.000 

FEET 

1.7258 

E-04 

LBS/CU.FT 

c 

150.000 

FEET 

,1.1468 

E-04 

LBS/CU.FT 

c 

160.000 

FEET  / 

7.8276 

E-05 

LBS/CU.FT 

c 

170.000 

FEET  f 
FEET? 

5.4467 

E-05 

LBS/CU.FT 

c 

160.000 

3.8700 

£-05 

lbs/cu.ft 

c 

190.009 

FEET 

2.7836 

E-05 

LBS/CU.FT 

c 

200.000 

FEET 

1.96S4 

E-05 

lbs/cu.ft 

c 

210.000 

FEET  ) 

1.3659 

E-05 

LBS/CU.FT 

c 

220.000 

FEET  / 

9.280? 

E-06 

LBS/CU.FT 

c 

230.000 

FEET 

6.1583 

E-06 

lbs/cu.ft 

c 

240.000 

FEET 

3.9784 

E-06 

LBS/CU.FT 

c 

250.000 

FEET 

2.493 

E-06 

LBS/CU.FT 

c 

260.000 

FEET 

1.508 

E-06 

LBS/CU.FT 

c 

270.000 

FEET 

8.343 

E-07 

LBS/CU.FT 

c 

260.000 

FEET 

4.522 

E-07 

LBS/CU.FT 

c 

290.000 

FEET 

2.453 

E-07 

LBS/CU.FT 

c 

300.000 

FEET 

1.327 

E-07 

lbs/cu.ft 

c 

310.000 

FEET 

6.880 

E-08 

LBS/CU.FT 

c 

.  426.600 

Ft£Y 

3.724 

£468 

Qft$/CU.ff 

c 

•330.000 

FEET 

2.093 

£-08 

LBS/CU.FT 

c 

340.000 

FEET 

1.216 

E-08 

LBS/CU.FT 

c 

c 

350.000 

2*000.000 

FEET 

FEET 

7.282 

0.000 

E-09 

L8S/CU.FT 

LBS/CU.FT 

C  THE  AIR  DENSITY  ABOVE  2.000.000  FEET  IS  ASSUMED  TO  BE  ZERO. 
C 


r 
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SUBROUTINE  ATMOS  I 

DIMENSION  PTABC631 *  ATABC63) .GTABC621 

DATA  ATAB/-1.0E4.-5.0E3.-1.0E3»0.0£0.1.0E3.2.0E3»4.0E3»6.0E3* 
18.0E3.1.0E4.1.2E4,1.4E4.1.6E4.1.8E4,2.0E4»2.2E4.2.4E4,2.6E4. 
22.8E4«3.0£4*3.2E4»3.4£4*3.6E4  *3.8E4»4»0E4*4.5E4*5.0E4»5.5E4* 
36.0E4.6.5E4,7.0E4»7#5E4*8.0E4»8.5E4*9«0E4»9.5E4»1.0E5»1»1E5* 
41,2E5.1.3E5»1.4E5.1.5E5.1.6E5.1.7E5,1.8E5»1.9E5.2.0E5.2.1E5. 
52.2E5.2.3E5,2.4E5«2.5£5.2.6E5.2.7E5,2.8E5.2.9E5»3.0E5,3.1E5. 
63.2E5.3.3E5.3.4E5.3.5E5.2.0E6/ 

DATA  PTAB/1.0150E-01.8.8310E-02.7.8730E-02.7.6475E-02.7*4262E-02. 
17.2099E-02.6.7918E-02.6.3926E-02.6.0116E-02»5.6483E-02*5.3022E-02, 
24.9725£-02»4.6589E-02.4.3606E-02.4.0773E-02*3.8083E-02.3.SS31E-02, 
33.3113E-02*3.0823E-02»2«8657E-02i2.6609E-02»2.4676E-02»2.2852E-02* 
42.0794E-02*1.8895E-02.1.4873£-02.1.1709£-C2*9.2ieSE-02,7.2588E-03» 
55.7164E-C3.4.5022E-03.3*5463£-03.2.7937E-03.2.1?64E-03.1.690lE-03. 
61.3182E-03.1.0332E-03.6.4392E-04.4.0851E-04.2.6349E-04.1.7258E-04. 
71.1468E-04.7.8276E-05.5.4467E-05.3.8700C-05.2.7836E-05.1.9664E-05. 
81.3659£-05«9.2S07£-0&»6« 1583E-06 .3»97B4E--06*2»4930E-06. 1.5080E-06. 
98.3430E-07.4.5?20E-07.2.4530E-07.1.3270E-07»6.8800E-08«3.7240E-08. 
12. 0930E-08 > 1.2160E-03  «7. 2820E-09  »0*0EC/ .K/l/ 

DO  10  1-1.62 

10  6TA6C I )=CPTA8C 1+1 1-PTA3C 1 17/ 1 ATABC 1  +  1 1-ATA3C 1 ) ) 

RETURN 

ENTRY  ATMOS ( H . RHO » PRriO 1 
IF  Cri  .GE.  ATABC 63 ) )  GO  TO  3 

1  IF  (H  -  ATA3CM+1 1 1  7.2.4 

2  RHO  =  PTA5IK+1) 

GO  TO  9 

3  RHO  =  0. 

PRHO*0.0 
GO  TO  9 

4  IF  (H  -  ATABCM+21 )  8.16.5 

5  M  *  M+l 
GO  TO  4 

6  M  *  M  ♦  1 
GO  TO  2 

7  M  *  M  -  1 
GO  TO  1 

3  RHO  *  PTABCM+l 1  +  (H  -  ATABCH+1 1 ) / CATABCM+2 )  -  ATABC M+l ))*(PTAB 
1CK+21  -  PTABCM+l 1) 

PRHO'GT  AB ( M+l 1  9 

9  RETURN 
END 
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IBFTC  INPUT.  DECK 

SUBROUTINE  INPUT-  READS  ALL  INPUT  DATA 

SUBROUTINE  INPUT 
COMMON  CI999) 

INTEGER  OUTNO.RNDMNO(S) 

DIMENSION  ONAME1 ( SO  I .0NAME2 ( SO) .OUTNOI  50 ) .LI STNOI 50) . V# L~£(  501  . 
EQUIVALENCE  .  (C ( *901 .NORNDM) ♦ ( C ( 499 ) • MOL 1ST  ). (CISCO) *\GOUT  I 

1  ( C ( SOI ) tONAME 1).(C(551) .ONAME2) »(C(6G!)«OJTNO  I 

2  (C(651I»L1STNO).(C(701). VALUE  ) •  (C(*S1 1 .RNDMNOl 
WR!TE{£»600) 

600  FORMAT  1 1H1 .AX.IGHINPUT  DATA//) 

100  READ  (5.500)  1R1 . ALPHA 1 >ALPtiA2 .ALPHAS* I R2 *VR1 »VR2 

500  FORMAT ( 12 « 3A6 .15.5X.2E15.0) 

WRITE (6. 601 1  1 Rl . ALPHA 1 .ALPHA2 • ALPUA3 » IR2 » VR1 »VR2 

601  FORMAT (5x, I2.3A6. 15.5X.1P2E15.7) 

GO  TO  (1.2.3. A. 5. 6) . I Rl 

1  GO  TO  ICO 

2  GO  TO  100 

3  C ( I R?  J - VR1 

IF (VR2.EC. 0.0)  60  TO  100 

NOLI ST=NOL I ST4 1 
LISTN0(NCLIST)-IR2 
VALUE (K0L1ST )-Vfil 
C-0  TO  100 
A  NOOUT=!:OOUT+1 

ONAME11NOOUT  )=ALPHA2 
ONAME2 (NOOUT ) =ALPHA3 
OUTNO(NOOUT)=IR2 
GO  TO  100 

5  GO  TO  100 

6  IF( IR2.EQ.OI  RETURN 
DO  7  1=1. IR2 

READ! 5.501)  J.X.NAME1 .NAME2. SIGMA, NAMES. NAME4.TAU 

501  FORMAT ( 1 5  *  El  5 .0.2 A5 .E15.0 *2A5 » E15. 0 ) 

WRITE (6.602)  J.X .NAMEl .NAME2. SIGMA, NAHE3.NAME4.TAU 

602  FORMAT (5X. I5»F15.3,2A5,1PE15.7»2A5,1PE15.7) 

NORNDM=NORNDM+ 1 

*  RNDMNO( I  1  =  J 
'  C(J)*X 

Cl  J-Fl )  =SIGMA 
'7  C(J*2)=TAU 
RETURN 
END 
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SIBFTC  OUPTI.  DECK 

SUBROUTINE  OUTPUT  —  OUTPUTS  DATA 

SUBROUTINE  OUPTI 
COMMON  C(999> 

INTEGER  OTCNT .PGCNT  »OUTNO 

DIMENSION  ONAMEl ( 50 1 .0NAME2)  SO  t  .OUTNO <  50  J .0 « 50 » 

EQUIVALENCE  ICI001I»T  )»tC(004)»CPP  )»(C(486) .PCNT  )» 

1(0487) (OTCNT  t « (0  486) .PGCNT  I  . (0489 ) » ITCNT  » • (0005) .DOC  I» 
2(C(500) .NOOUT  ) .  (C( 5011 .OMAME1 ) *(0(5511 .ONAKE2) »(CC601 ) .OUTNO  ) 
1TCNT  *  DOC  ♦  1.0 
PCHT  *  J-O.OOOGOi 
PBCNT  *  1 

OTCNT  ■  (NOOUT  ♦  4)/5 
CO  TO  100 
ENTRY  OUTPUT 

100  IFUTCNT.GT.6)  CO  TO  1 

ITCNT-ITCNT+1  « 

WRITE  (6«600)  1 1  »C(  I) .01+1 1.0 1*2) »C(I93) «C( 1+4) »C( 1+5 1.0 1+6) » 
1  01+7). 1*1. 472. 8) 

600  FORMAT ( 1H1  »5X .14MC0HM9M  L1STING/II5.2X.1P8E15.7) ) 

P6CHT=1 

1  IFIT.LT.PCNT)  RETURN 
PCKT=PCNT+CPP 
IF(PGCNT.NE.l)  CO  TO  3 

2  WRI TE (6.601 )  ( ONAME 1(1) .ONAKE 2(I>.X— 1. NOOUT ) 

601  FORMAT  )1H1.5X»4MT1M?:,5)(,5I8X.2A6)/  (23X.2A6.8X.2A6.8X.2A6.8X. 
12A6.8X.2A6)  ) 

PGCNT*2*DTCNT+4 

3  IF(PGCNT.GE.62)  CO  TO  2 
DO  4  1-1. NOOUT 
J*OUTNO( I ) 

4  B( I )*C( J) 

WRITE) 6.603)  T,(B( I ) .1-1 .NOOUT) 

603  FORMAT)/// /2X.F15.7.1P5E20.7/I17X.1P5E20. 7)) 

PGCNT  »  PCCNT  +  OTCNT  ♦  4 
RETURN 
END 


:  RESEf.  DECK 

SUBROUTINE  RESET  RESETS  SELECTEJ)  INPUT  DATA  FOR  REPEATED  RUNS, 

SUBROUTINE  RESET 
t  COMMON  0999)  '  /  . 

EQUIVALENCE  (C( 499) .NOLI St) . (C (651 ) .LI STNO) . (C (701 ) .VALUE  ) 
DIMENSION  LI STNO ( 50 ) .VALUE ( 50  T 
IF  (NOLIST  .EQ.  0)  RETURN 
DO  i  I  *  1.  NOLIST 
J  *  IISTNO(I) 

1  C(J)  *  VALUE! I) 

RETURN 

END 
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iieFTC  MFSDO  oecu 


subroutine  hfsb 
:  purpose 

FACTOR  A  GIVEN  SYMMETRIC  POSITIVE  DEFINITE  MATRIX 


USAGE 

CALL  MFSDt A.N.EPS.IER) 
DESCRIPTION  OF  PARAMETERS 


A  -  UPPER  TRIANGULAR  PART  OF  THE  GIVEN  SYMMETRIC  MFSC 

POSITIVE  DEFINITE  N  BY  N  COEFFICIENT  MATRIX.  MFSD 

ON  RETURN  A  CONTAINS  THE  RESULTANT  UPPER  MFSD 

TRIANGULAR  MATRIX.  MFSD 

N  -  THE  NUMBER  OF  ROWS  (COLUMNS)  IN  GIVEN  MATRIX.  MFSD 

EPS  -  AN  INPUT  CONSTANT  WHICH  IS  USED  AS  RELATIVE  MFSD 

,  TOLERANCE  .FOR  TEST  ON  LOSS  OF  SIGNIFICANCE.  MFSD 

IER  -  RESULTING  ERROR  PARAMETER  CODED  AS,  FOLLOWS  MFSD 

I£R"0  —  NO  ERROR  '  MFSD 

!ER=-1  -  NO  RESULT  BECAUSE  OF  WRONG  INPUT  PARAME-  MFSD 
TER  N  OR  BECAUSE  SOKE  RADICAND  IS  NON-'  MFSD 
POSITIVE  (MATRIX  A  IS  NOT  POSITIVE  MFSD 

DEFINITE.  POSSIBLY  DUE  TO  LOSS  OF  SIGNI-  MFSD 
,  F1CANCEI  MFSD 

!ER=K  -  WARNING  WHICH  INDICATES  LOSS  OF  SIGNIFI-*  MFSD 
CANCE.  THE  RADICAND  FORMED  AT' FACTOR IZA-  MFSD 
TION  STEP  K+l  WAS  STILL  POSITIVE  BUT  NO  MFSD 
LONGER  6REATER  THAN  ABS(EP£*A<K+1»K+1> I.  MFSD 

MFSD 

REMARKS  MFSD 

THE  UPPER  TRIANGULAR  PART  OF  GIVEN  MATRIX  IS  ASSUMED  TO  BE  MFSD 
STORED  COLUMNWISE  IN  N*4N+l)/2  SUCCESSIVE  STORAGE  LOCATIONS.MFSD 
IN  THE  SAME  ST0RA6E  LOCATIONS  THE  RESULTING  UPPER  TRIAftGU-  MFSD 
LAR  MATRIX  IS  STORED  COLUMNWISE  TOO.  i  MFSD 

THE  PROCEDURE  GIVES  RESULTS  IF  N  IS  GREATER  THAN  0  AMD  ALL  MFSD 
CALCULATED  RADI CANOS-  ARE  POSITIVE.  1  MFSD 

"THE 'PRODUCT  OF  RETURNED  DIAGONAL  TERMS  IS  EQUAL  TO  THE  MFSD 

SQUARE-ROOT  OF  THE  DETERMINANT  OF  THE  GIVEN  MATRIX.  MFSD 

MFSD 

SUBROUTINES  AND  FUNCTION  SUBPROGRAMS  REQUIRED  MF5D 

NONE  -  %  MFSD 

MFSD 

METHOD  \  MFSD 

SOLUTION  IS  DONE  USTNG  THE  SOUARE-ROOT  METHOD  OF  CHOLESKY.  MFSD 
THE  GIVEN  MATRIX  IS  REPRESENTED  AS  PRODUCT  OF  TWO  TRIANGULARMFSD 
MATRICES.  WHERE  THE  LEFT  HAND  FACTOR  IS  THE  TRANSPOSE  OF  MFSD 
THE  RETURNED  HIGMT  HAND  FACTOR.  •  MFSD 

/  MFSD 


MFSD 

10  I 

.MFSD 

20  \ 

MFSD 

30 

MFSD 

40 

HFSO 

50. _ 

Tcfsd 

60 

MFSD 

70 

MFSD 

80 

MFSD 

90 

MFSD 

100 

MFSD 

110 

MFSD 

120 

MFSD 

130 

MFSD 

140 

MFSD 

150 

MFSD 

160 

MFSD 

170 

MFSD 

180 

MFSD 

190 

MFSD 

200 

MFSD  21Q 

MFSD 

220 

MFSD 

230 

MFSD 

240 

MFSD 

250 

MFSD 

260 

MFSD 

270 

MFSD 

280 

MFSD 

290 

MFSD 

300 

MFSD 

310 

MFSD 

32C 

MFSD 

330 

.MFSD 

340 

MFSD 

350 

MFSD 

360 

MFSD 

370 

MFSD 

380 

MFSD 

390 

MFSD  40 0 

MFSD 

410 

MFSD  420 
MFSD  430 
MFSD  440 
MFSD  450 
MFSD  460 
IMF  SO  470 
MFSD  480 
MFSD  490 
MFSD  500 


SUBROUTINE  Mr SO (A.N. EPS. IER) 


DIMENSION  AC  1 > 

DOUBLE  PRECISION  DPIV.OSUM 

TEST  ON  WRONG  INPUT  PARAMETER  N 
IFIN-l I  12.1.1 
1  1ER«0 

INITIALIZE  0 I AGONAL-LOOP 
KPIV*0 
DO  11  X-l.N 

ip!V*x^lv+ic 

IND-KPIV 
LENOXX- 1 

CALCULATE  TOLERANCE 
T0L-A6S(EPS»A(KP!V>) 


520 
530 
540 
550 
560 
570 
580" 
590 
600.  , 
610 
620 
630 
640 
650 
660 
670 
680 
690 
700 
710 
720 


« 
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START  FACTOR IZATIOM-LOOP  OVER  K-TH  ROW 
00  11  !*K»N 
OSUM'O.DO 

TFTLENPI  2*4*2  / 

START  INNER  LOOP  ’ 

00  3  L*1»LEND  x  / 

LANF=RPIV~L 

,  OSOM=OSUM+06LECAILANf »*AIL1ND1I  / 

END  OF  INNER  LOOP 

TRANSFORM  ELEMENT  A(1ND> 

>  OSUM=DBLEI A( INDI  I-DSUM 
IFtl-Kl  10*5*10 

TEST  FOR  NEGATIVE  PIVOT  ELEMENT  AMD  FOR  LOSS  OF  SIGNIFICANCE 
»  IF ( SN6L<DS"K)-T0L J  6*6*9 

,  IF CDSUMI  12*12*7  / 

r  IFUERI  8*6*9,—  /  / 

J  IEP.=n-l  /  ' 

COMPUTE  PIVOT  ELEMENT 

9  DPIV=OSQRT«OSU»!)  ' 

A(KPIVI*DPIV 
DPI V=l* DO/DP IV 
GO-  TO  11 

‘  .CALCULATE  TERMS  IN  ROW, 

0  A(IK0I*DSUM»0P1V 

1  IND=INO+I 

END  OF  DIAGONAL-LOOP  , 

RETURN  C 

2  IER*-1  /  * 

RETURN  •  _ _ _ , 

ENO  ^  ~  -  - 


MFSD  730 
MFSO  740 
MFSO  750 
MFSO  760 
MFSO  770 
MFSO  760 
MFSO  790 
MFSO  800 
MFSO  610 
MFSO  820 
MFSD  630 
MFSO  840 
MFSO  850 
MFSO  860 
MFSD  870 
KFSD  88 0 
MFSD  890 
MFSD  900 
MFSD  910 
MFSD  920 
MFSO  930 
MFSD  940 
MFSD  950 
MFSD  960 
MFSD  970 
MFSD  960 
MFSD  990 
MFSDlOOO 
MFSO  10 10 
MFSD1020 
MFSD1030 
MFSD1040 
MFSD 10 50 
HFSD1060 
MFS01070 
MFS01080 
_ _ JIFS01090 


i 
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SIBFTC  S1NV0 

C 


SI  XV  10 


SUBROUTINE  SIHV 

PURPOSE  *, 

INVERT  A  GIVEN  SYMHETRK  POSITIVE  DEFINITE  MATRIX 

USAGE 

CALL  SINV(A.K.EPS.IER) 

DESCRIPTION  OF  PARAMETERS 

A  _  UPPER  TRIANGULAR  PART  OF  THE  GIVEN  SYMMETRIC 

POSlTrVE-~DEFJfH-T£ Jl_BY  N  COEFFICIENT  MATRIX. 

ON  RETURN  A  CON*  NSlHT~ReSULIAIlljJPPER 

TRIANGULAR  MATRiX.  — — — 

N  -  THE  NUMBER  OF  ROWS  (COLUMNS}  IN  GIVEN  MATRIX. 

EPS  -  AN  INPUT  CONSTANT  WHICH  IS  USED  AS  RELATIVE  - 
TOLERANCE  T'OR  TEST  ON  LOSS  OF  SIGNIFICANCE. 

IER  -  RESULTING  ERRCR  PARAMETER  CODED  AS  FOLLOWS 
!£R=<K  -  NO  ERROR  * 

IER*~1  -  NO  RESULT  BECAUSE  OF  WRONG  INPUT  PARAME- 
/  TER  N  OR  BECAUSE  SOKE  RAD I C AND  IS  NON- 

/  POSITIVE  (MATRIX  A  IS  NOT  POSITIVE 

/  DEFINITE.  POSSIBLY  DUE  TO  LOSS  OF  SIGM- 

’  F1CANCE) 

IER=K  -  WARNING  WHICH  INDICATES  LOSS  OF  S1GNIF1- 
/'  CANCE.  THE  RADICAND  FORMED  AT^FACTORIZA- 

T10N  STEP  K+l  WAS  STILL  POSITIVE  BUT  NO 
LONGER  GREATER  THAN  ABS(EPS*A(X+l.X+lI j. 


IER=R  - 


SINV  30 
SINV  40 
SINV  SO 
SINV  SO 
SINV.  70 
SINV  BO 
SINV  90 
SINV  100 
SINV  110 
SINV  120 
SINV  130 
SINV  140 
SINV  ISO 
SINV  160 
SINV  170 
SINV  180 
SINV  190 
SINV  200 
SINV  210 
SINV  220 
SINV  23C 
SINV  240 
SINV  2S0 
SINV  260 
SINV  270 
SINV  280 
SINV  290 
SINV  300 
SINV  310 
SINV  320 
SINV  330 


REMARKS  SINV  320 

THE  UPPER  TRIANGULAR  PART  OF  GIVEN  MATRIX  IS  ASSUMED  TO  BE  SINV  330 
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